Hello,
I am using a turtlebot 2 with ros-groovy. I have successfully wrote a publisher that takes the base_link and broadcasts that info. I am working on writing a broadcaster for the base_footprint. Also my base_link broadcaster works perfect. That is why I just followed the same format for that. But I don't understand why this logic is happening.
The base_foot print broadcaster seems to be messing up though. The broadcaster is able to keep the correct distance but when I start turning the robot things go bad. What goes wrong is that as the robot starts turning the base_footprint frame starts revolving around the fixed map frame (0,0,0). What I would expect is that whenever the turtlebot turns in the z direction that the base_footprint stays with the robot model on rviz like it is suppose to. If anyone could help it would be appreciated.
I have 2 examples there of what Is going on so you can get an idea.
while(ros::ok()){
tf::StampedTransform transform; //for the base_link
tf::StampedTransform transformFP; //for the base_footprint
current_time = ros::Time::now();
//This is for the base_link frame which works great this block of code just gets the transform and that transform is
//broadcasted as leader_base_link to the tf tree.
try {
listener.waitForTransform("base_link", "base_footprint", ros::Time(0), ros::Duration(10.0));
listener.lookupTransform("base_link", "base_footprint", ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
}
yaw = tf::getYaw(transform.getRotation());
v.setValue(transform.getOrigin().x() , transform.getOrigin().y() , transform.getOrigin().z());
leader_base_link.setOrigin(v);
q.setRPY(0.0, 0.0, yaw);
leader_base_link.setRotation(q );
br.sendTransform(tf::StampedTransform(leader_base_link, ros::Time::now(), "base_footprint", "leader_base_link"));
double th = yaw;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
transformConverter.header.stamp = current_time;
transformConverter.header.frame_id = "base_footprint";
transformConverter.child_frame_id = "base_link";
transformConverter.transform.translation.x = transform.getOrigin().x();
transformConverter.transform.translation.y = transform.getOrigin().y();
transformConverter.transform.translation.z = transform.getOrigin().z();
transformConverter.transform.rotation = odom_quat;
/*This is where the base_footprint is being created I also believe this is where the code is messing up.
As the robot turns in the yaw angle the position of the base_footprint starts translating into a + or - direction but the distance is kept . As you can see in the picture that is what happens when the robot turns in a 180 degrees. The base_footprint gets shifted in 180 degrees This is following the same exact code as the base_link that works fine as well*/
try {
listener.waitForTransform("base_footprint", "odom", ros::Time(0), ros::Duration(10.0));
listener.lookupTransform("base_footprint", "odom", ros::Time(0), transformFP);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
}
*/ double tryYaw = atan2( (2*transformFP.getRotation().w()*transformFP.getRotation().z() ), (transformFP.getRotation().w()*transformFP.getRotation().w() - transformFP.getRotation().z()*transformFP.getRotation().z())); This is another equation I tried but it basically does the same exact stuff. */
//this is where the yaw angle gets calculated.
yaw2 = tf::getYaw(transformFP.getRotation());
v.setValue(transformFP.getOrigin().x() , transformFP.getOrigin().y() , transformFP.getOrigin().z());
leader_footprint.setOrigin(v);
q.setRPY(0.0, 0.0, yaw2);
leader_footprint.setRotation(q );
//this is where the leader_footprint gets added to the transform tree
br.sendTransform(tf::StampedTransform(leader_footprint, ros::Time::now(), "odom", "leader_base_footprint"));
//The rest of the code just used to be able to send the StampedTransform over a topic Don't worry too much about this part
double th2 = yaw2;
geometry_msgs::Quaternion odom2_quat = tf::createQuaternionMsgFromYaw(th2);
transformConverterFP.header.stamp = current_time;
transformConverterFP.header.frame_id = "odom";
transformConverterFP.child_frame_id = "base_footprint";
transformConverterFP.transform.translation.x = transformFP.getOrigin().x();
transformConverterFP.transform.translation.y = transformFP.getOrigin().y();
transformConverterFP.transform.translation.z = transformFP.getOrigin().z();
transformConverterFP.transform.rotation = odom2_quat;
ROS_INFO_STREAM("X pose " << transformConverterFP.transform.translation.x );
ROS_INFO_STREAM("Y pose " << transformConverterFP.transform.translation.y);
ROS_INFO_STREAM("Z pose " << transformConverterFP.transform.translation.z );
ROS_INFO_STREAM("Yaw Interposed : "<< yaw2 );
ROS_INFO_STREAM("Yaw Z "<< transformFP.getRotation().w() );
ROS_INFO_STREAM("Yaw Z "<< transformFP.getRotation().z() );
pub.publish(transformConverter);
pubFoot.publish(transformConverterFP);
loopRate.sleep();
}
↧