tf tutorial - problem in rqt_graph
I did tf tutorials : Writing tf broadcaster and listener .
It went well. Then I tried to modify the code: I added a 3rd turtle that will follow 2nd turtle. So the scenario will be : The user control turtle1 using keyboard || turtle2 follow turtle1 || turtle 3 follow turtle 2.
My code worked well and did what it supposed to do.(that is turtle 3 catching turtle 2 catching turtle 1) HOWEVER, when I tried to look at the rqt_graph, using rosrun rqt_graph rqt_graph , the graph did not show what I expected.
I think the the listener must publish /turtle2/cmd_vel to sim, but rqt_graph did not show so.
QUESTION: Can you please explain why or is there anything wrong (but the code worked well) ?
These are my codes: PS: I add "_mon" to all stuffs, so the names don't clash, cause I still keep the original files
start_demo_mon_launch
<launch> <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<node pkg="learning_tf" type="turtle_tf_broadcaster_mon"
args="/turtle1" name="turtle1_tf_broadcaster_mon" />
<node pkg="learning_tf" type="turtle_tf_broadcaster_mon"
args="/turtle2" name="turtle2_tf_broadcaster_mon" />
<node pkg="learning_tf" type="turtle_tf_broadcaster_mon"
args="/turtle3" name="turtle3_tf_broadcaster_mon" />
<node pkg="learning_tf" type="turtle_tf_listener_mon"
name="listener_mon" />
</launch>
turtle_tf_listener_mon.cpp
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv); //spawn turtle2
srv.request.x=5;
add_turtle.call(srv); //spawn turtle3
//publisher to control turtle 2
ros::Publisher turtle_vel_2 =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
//publisher to control turtle 3
ros::Publisher turtle_vel_3 =
node.advertise<geometry_msgs::Twist>("turtle3/cmd_vel", 10);
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform_12;
tf::StampedTransform transform_23;
try{
listener.lookupTransform("/turtle2", "/turtle1",ros::Time(0), transform_12);
listener.lookupTransform("/turtle3", "/turtle2",ros::Time(0),transform_23);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
//send velocitry command to turtle 2
geometry_msgs::Twist vel_msg_2;
vel_msg_2.angular.z = 4.0 * atan2(transform_12.getOrigin().y(),
transform_12.getOrigin().x());
vel_msg_2.linear.x = 0.5 * sqrt(pow(transform_12.getOrigin().x(), 2) +
pow(transform_12.getOrigin().y(), 2));
turtle_vel_2.publish(vel_msg_2);
//send velocitry command to turtle 3
geometry_msgs::Twist vel_msg_3;
vel_msg_3.angular.z = 4.0 * atan2(transform_23.getOrigin().y(),
transform_23.getOrigin().x());
vel_msg_3.linear.x = 0.5 * sqrt(pow(transform_23.getOrigin().x(), 2) +
pow(transform_23.getOrigin().y(), 2));
turtle_vel_3.publish(vel_msg_3);
rate.sleep();
}
return 0;
};
turtle_tf_broadcaster_mon.cpp I didn't edit anything here, except the naming (adding "_mon" for consistency)
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta ...