tf tutorial - problem in rqt_graph

asked 2016-08-25 04:03:27 -0600

alienmon gravatar image

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) ?

image description

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 ...
(more)
edit retag flag offensive close merge delete