Subscriber not seeing cmd_vel messages
Hi,
I've written a node with publishes (Successfully) and subscribes to cmd_vel messages. Problem I'm getting is that the call back is never being called. C++ code is very simple, as per tutorial - abridged version below:
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/LaserScan.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <std_msgs/Bool.h>
#include <diagnostic_updater/publisher.h>
#include <geometry_msgs/Twist.h>
// For moving the robot on the map
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "Consts.h"
#include "RobotInterface.h"
#include "NetworkInterface.h"
void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd)
{
ROS_INFO("I heard: [%s]", vel_cmd.linear.y);
cout << "Twist Received " << endl;
}
int main( int argc, char* argv[] )
{
ros::init(argc, argv, "toeminator_publisher" );
ros::NodeHandle n_("~");
ros::NodeHandle n("");
ros::NodeHandle lSubscriber("");
ros::NodeHandle nIR("");
ros::NodeHandle nLaser("");
ros::Rate loop_rate(10);
ros::Subscriber sub = lSubscriber.subscribe("/cmd_vel", 1000, cmd_vel_callback);
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
RobotData lRobotData;
while( n.ok() )
{
ros::spin();
}
return 1;
}
The python publisher is basically the turtlebot's keyboard publisher and running rtq_graph shows the message link between the publisher and subscriber as expected (below) and "rostopic echo /cmd_vel" shows the message flowing
Not sure where I've gone wrong here. Would welcome any pointers.
Kind Regards
Mark
EDIT: Updated with full code. Also I neglected to mention (apologies) that the roscore and the C++ processes are running on a different machine to where the teleop keyboard process is running. That said publishing from the roscore machine to rviz etc on the telop machine is working with out issue (ROS_IP is set to the IP addresses of the respective machines - as mentioned in the networking setup tutorials)
Can you please copy the rest of the code? That can help