sumant's profile - activity

Specification: Ubuntu 16.04, ROS kinetic, Gazebo, turtlebot3

Here is my rqt_graph

There is a node /negotiator which should subscribe cca_cmd_vel and publish to cmd_vel.

turtlebot3_teleop_keyboard and move_base publish on cca_cmd_vel.

Here is the code

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

void cca_cmd_velCallback(const geometry_msgs::Twist::ConstPtr& vel)
{
  geometry_msgs::Twist new_vel = *vel;
  cmd_vel_pub.publish(new_vel);
}

int main(int argc, char **argv)
{
//Negotiator Subscribe cmd_vel

  ros::init(argc, argv, "negotiator");
  ros::NodeHandle n;


  ros::Subscriber sub = n.subscribe("cca_cmd_vel", 1000, cca_cmd_velCallback);


//Negotiator Publish to GAZEBO
  ros::Publisher cmd_vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
//ros::Publisher cmd_vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
ros::spin();

  return 0;
}

Kindly suggest where to improve

Few More add on information

crlab@crlab-500-100IX:~$ rostopic info cca_cmd_vel

Type: geometry_msgs/Twist

Publishers: 
 * /move_base (http://localhost:34459/)
 * /turtlebot3_teleop_keyboard (http://localhost:41875/)

Subscribers: None

crlab@crlab-500-100IX:~$ rostopic info cmd_vel

Type: geometry_msgs/Twist

Publishers: None

Subscribers: 
 * /gazebo (http://localhost:33175/)
 * /negotiator (http://localhost:35659/)

crlab@crlab-500-100IX:~$ rosnode info negotiator

--------------------------------------------------------------------------------
Node [/negotiator]
Publications: 
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /clock [rosgraph_msgs/Clock]
 * /cmd_vel [unknown type]

Services: 
 * /negotiator/get_loggers
 * /negotiator/set_logger_level


contacting node http://localhost:35659/ ...
Pid: 4967
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /clock
    * to: /gazebo (http://localhost:33175/)
    * direction: inbound
    * transport: TCPROS

crlab@crlab-500-100IX:~$ rosnode info turtlebot3_teleop_keyboard

--------------------------------------------------------------------------------
Node [/turtlebot3_teleop_keyboard]
Publications: 
 * /cca_cmd_vel [geometry_msgs/Twist]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /clock [rosgraph_msgs/Clock]

Services: 
 * /turtlebot3_teleop_keyboard/get_loggers
 * /turtlebot3_teleop_keyboard/set_logger_level


contacting node http://localhost:41875/ ...
Pid: 4938
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /clock
    * to: /gazebo (http://localhost:33175/)
    * direction: inbound
    * transport: TCPROS

crlab@crlab-500-100IX:~$ rosnode info move_base

Node [/move_base]
Publications: 
 * /cca_cmd_vel [geometry_msgs/Twist]
 * /move_base/DWAPlannerROS/cost_cloud [sensor_msgs/PointCloud2]
 * /move_base/DWAPlannerROS/global_plan [nav_msgs/Path]
 * /move_base/DWAPlannerROS/local_plan [nav_msgs/Path]
 * /move_base/DWAPlannerROS/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/DWAPlannerROS/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/DWAPlannerROS/trajectory_cloud [sensor_msgs/PointCloud2]
 * /move_base/NavfnROS/plan [nav_msgs/Path]
 * /move_base/current_goal [geometry_msgs/PoseStamped]
 * /move_base/feedback [move_base_msgs/MoveBaseActionFeedback]
 * /move_base/global_costmap/costmap [nav_msgs/OccupancyGrid]
 * /move_base/global_costmap/costmap_updates [map_msgs/OccupancyGridUpdate]
 * /move_base/global_costmap/footprint [geometry_msgs/PolygonStamped]
 * /move_base/global_costmap/inflation_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/global_costmap/inflation_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/global_costmap/obstacle_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/global_costmap/obstacle_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/global_costmap/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/global_costmap/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/global_costmap/static_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/global_costmap/static_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/goal [move_base_msgs/MoveBaseActionGoal]
 * /move_base/local_costmap/costmap [nav_msgs/OccupancyGrid]
 * /move_base/local_costmap/costmap_updates [map_msgs/OccupancyGridUpdate]
 * /move_base/local_costmap/footprint [geometry_msgs/PolygonStamped]
 * /move_base/local_costmap/inflation_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/local_costmap/inflation_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/local_costmap/obstacle_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/local_costmap/obstacle_layer/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/local_costmap/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/local_costmap/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /move_base/parameter_updates [dynamic_reconfigure/Config]
 * /move_base/result [move_base_msgs/MoveBaseActionResult]
 * /move_base/status [actionlib_msgs/GoalStatusArray]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /clock [rosgraph_msgs/Clock]
 * /map [nav_msgs/OccupancyGrid]
 * /move_base/cancel [unknown type]
 * /move_base/global_costmap/footprint [geometry_msgs/PolygonStamped]
 * /move_base/goal [move_base_msgs/MoveBaseActionGoal]
 * /move_base/local_costmap/footprint [geometry_msgs/PolygonStamped]
 * /move_base_simple/goal [geometry_msgs/PoseStamped]
 * /odom [nav_msgs/Odometry]
 * /scan [sensor_msgs/LaserScan]
 * /tf [tf2_msgs/TFMessage]
 * /tf_static [tf2_msgs/TFMessage]

Services: 
 * /move_base/DWAPlannerROS/set_parameters
 * /move_base/NavfnROS/make_plan
 * /move_base/clear_costmaps
 * /move_base/get_loggers
 * /move_base/global_costmap/inflation_layer/set_parameters
 * /move_base/global_costmap/obstacle_layer ...

I wish to interface ROS-USARSim. and using a interface package executive_usarsim.

It spawn P3AT in USARSim but while running rosrun teleop_twist_keyboard teleop_twist_keyboard.py

P3AT do not move.

Actually in rqt_graph; executive_usarsim interface donot subscribe twist messages;

Therefore i wish to write a cpp node which can subscribe /cmd_vel topic and publish it on /executive_usarsim/status topic.

please help how to write it in a single cpp file.

Here is rosnode info executive_usarsim

 cr-lab-tu@crlabtu-HP-Compaq-8100-Elite-SFF-PC:~$ rosnode info executive_usarsim
 --------------------------------------------------------------------------------
 Node [/executive_usarsim]
 Publications: 
  * /executive_usarsim/status [std_msgs/String]
  * /rosout [rosgraph_msgs/Log]

 Subscriptions: 
  * /executive_usarsim/status [std_msgs/String]

 Services: 
  * /executive_usarsim/get_loggers
  * /executive_usarsim/set_logger_level


 contacting node http://crlabtu-HP-Compaq-8100-Elite-SFF-PC:45055/ ...
 Pid: 12463
 Connections:
  * topic: /executive_usarsim/status
     * to: /executive_usarsim
     * direction: outbound
     * transport: TCPROS
  * topic: /rosout
     * to: /rosout
     * direction: outbound
     * transport: TCPROS
  * topic: /executive_usarsim/status
     * to: /executive_usarsim (http://crlabtu-HP-Compaq-8100-Elite-SFF-PC:45055/)
     * direction: inbound
     * transport: TCPROS

and rosnode info teleop_twist_keyboard

 cr-lab-tu@crlabtu-HP-Compaq-8100-Elite-SFF-PC:~$ rosnode info teleop_twist_keyboard
 --------------------------------------------------------------------------------
 Node [/teleop_twist_keyboard]
 Publications: 
 * /rosout [rosgraph_msgs/Log]
 * /cmd_vel [geometry_msgs/Twist]

 Subscriptions: None

 Services: 
 * /teleop_twist_keyboard/set_logger_level
 * /teleop_twist_keyboard/get_loggers


 contacting node http://crlabtu-HP-Compaq-8100-Elite-SFF-PC:55529/ ...
 Pid: 12620
 Connections:
  * topic: /rosout
      * to: /rosout
      * direction: outbound
      * transport: TCPROS

EDIT1:

here is the code i have written to solve the above issue,

  #include <ros/ros.h>
 #include "std_msgs/String.h"
 #include <sstream>
 class SubscribeAndPublish
 {
 public:
   SubscribeAndPublish()
   {
     //Topic you want to publish
     pub_ = n_.advertise<std_msgs::String>("/executive_usarsim", 1);

     //Topic you want to subscribe
     sub_ = n_.subscribe("/cmd_vel", 1, &SubscribeAndPublish::callback, this);
   }

   void callback(const std_msgs::String::ConstPtr& input)
   {
     std_msgs::String output;
     //output=input;
     //.... do something with the input and generate the output...
     pub_.publish(input);
   }

 private:
   ros::NodeHandle n_; 
   ros::Publisher pub_;
   ros::Subscriber sub_;

 };//End of class SubscribeAndPublish

 int main(int argc, char **argv)
 {
   //Initiate ROS
   ros::init(argc, argv, "sub-pub");

   //Create an object of class SubscribeAndPublish that will take care of everything
   SubscribeAndPublish SAPObject;

   ros::spin();

   return 0;
 }

But it is only publishing and not subscribing.

bcrlab@bcrlab-HP-Compaq-8100-Elite-SFF-PC:~$ rosnode info sub_pub
--------------------------------------------------------------------------------
Node [/sub_pub]
Publications: 
 * /executive_usarsim/status [std_msgs/String]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /cmd_vel [geometry_msgs/Twist]

Services: 
 * /sub_pub/set_logger_level
 * /sub_pub/get_loggers


contacting node http://bcrlab-HP-Compaq-8100-Elite-SFF-PC:33082/ ...
Pid: 21939
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound
    * transport: TCPROS
 * topic: /executive_usarsim/status
    * to: /executive_usarsim
    * direction: outbound
    * transport: TCPROS

and

bcrlab@bcrlab-HP-Compaq-8100-Elite-SFF-PC:~$ rostopic info \cmd_vel
Type: geometry_msgs/Twist

Publishers: 
 * /teleop_twist_keyboard (http://bcrlab-HP-Compaq-8100-Elite-SFF-PC:45082/)

Subscribers: 
 * /sub_pub (http://bcrlab-HP-Compaq-8100-Elite-SFF-PC:33082/)

EDIT2:

Here is the updated code which work for me,

#include <ros/ros.h>
#include "std_msgs/String.h"
#include <sstream>
#include <geometry_msgs/Twist.h>
class SubscribeAndPublish
{
public:
  SubscribeAndPublish()
  {
    //Topic you want to publish
    pub_ = n_.advertise<geometry_msgs::Twist>("/executive_usarsim/pub_tele", 1);

    //Topic you want to subscribe
    sub_ = n_.subscribe("/cmd_vel", 1, &SubscribeAndPublish::callback, this);
  }

  void callback(const geometry_msgs::Twist::ConstPtr& input)
  {
    geometry_msgs::Twist output;
    //output=input;
    //.... do something with the input and generate the output...
    pub_.publish(input);
  }

private:
  ros::NodeHandle n_; 
  ros::Publisher pub_;
  ros::Subscriber sub_;

};//End of class SubscribeAndPublish

int main(int argc, char **argv)
{
  //Initiate ROS
  ros::init(argc, argv, "sub_pub");

  //Create an object of class SubscribeAndPublish that will take care of everything
  SubscribeAndPublish SAPObject;

  ros::spin();

  return 0;
}

But now the issue is ... (more)