Pablo's pioneer_tf.cpp

Renato shared with me a post of a ROS user that does slam using the ROSARIA node. Here is the url where this post can be found and following the program itself can be found:

http://ros-users.122217.n3.nabble.com/Pioneer3-td1265101.html#a1288712




#include
#include
#include



void poseCallback(const nav_msgs::Odometry::ConstPtr& odomsg)
{
//TF odom=> base_link

static tf::TransformBroadcaster odom_broadcaster;
odom_broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(odomsg->pose.pose.orientation.x,
odomsg->pose.pose.orientation.y,
odomsg->pose.pose.orientation.z,
odomsg->pose.pose.orientation.w),
tf::Vector3(odomsg->pose.pose.position.x/1000.0,
odomsg->pose.pose.position.y/1000.0,
odomsg->pose.pose.position.z/1000.0)),
odomsg->header.stamp, "/odom", "/base_link"));
ROS_DEBUG("odometry frame sent");
}




int main(int argc, char** argv){
ros::init(argc, argv, "pioneer_tf_publisher");
ros::NodeHandle n;

ros::Rate r(20);

tf::TransformBroadcaster broadcaster;

//subscribe to pose info
ros::Subscriber pose_sub = n.subscribe("RosAria/pose", 1, poseCallback);

while(n.ok()){
//base_link => laser
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0), tf::Vector3(0.13, -0.04, 0.294)),
ros::Time::now(), "/base_link", "/laser"));

ros::spinOnce();
r.sleep();
}
}