ROS+RViz: Husky p-controller #2

The marker location is determined from the laser scan data which is in the reference frame of the base_laser. Technically, RViz will do the conversion for us, but as an exercise, was worth transforming the location in the base_laser frame to that of odom before publishing data for use by RViz.

The steps taken were as follows:

Include the necessary headers

Include relevant header files declaring the necessary message types.

...
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
...

Declare the buffer for storage

Declare the buffer, and listener in the header file. The buffer stores the transform related data, and has the public member function lookupTransform that we will be using to get the transform between two frames.

tf2_ros::Buffer tfBuffer_;
tf2_ros::TransformListner listener_ {tfBuffer_};

Declare relevant messages

Declare a geometry_msgs::TransformStamped message, transformStamped, and use the lookupTransform() function to get the transform. The point here is that the frames dont need the ‘/’ in front, and the first parameter is the frame that we want to transform to or said differently, the target frame. The full specification of the function can be found here.

geometry_msgs::Transformed transformStamped;
try {
    transformStamped = tfBuffer_.lookupTransform("odom", "base_laser", ros::Time(0));
} catch (tf2::TransformExcetion &ex) {
    ROS_WARN("%s", ex.what());
    ros::Duration(1.0).sleep();
}

“Stamp” the data

Finally we need to “stamp” the source point before passing to the transform() function. The api of geometry_msgs/PoseStamped can be found here.

geometry_msgs::PoseStamped pose_in;
geometry_msgs::PoseStamped pose_out;

pose_in.pose.position.x = min*cos(-theta);
pose_in.pose.position.y = min*sin(-theta);
pose_in.header.stamp = ros::Time(0);
pose_in.header.frame_id = "base_laser";

tfBuffer_.transform(pose_in, pose_out, "odom");

Now we have the location of the pillar originally in the /base_laser frame transformed to the /odom frame.

The full source code can be found here, under ros_pkg/git/husky_highlevel_controller/src/HuskyHighlevelController.cpp

Dont forget to make the necessary changes to the CMakefile and package.xml files.

ROS Wiki

Also, found small issues with the ROS Wiki thus made a quick edit. Basically the prior post had the specifications of the parameters to lookupTransform() described incorrectly.

References

ROS Wiki, tf2 Tutorials. Writing a tf2 listener.
Fankhauser, P., Dominic, J., and Wermelinger, Martin.(2017) Course 3, Programming for Robotics (ROS).