Subscribing and publishing geometry/Twist messages from Turtlesim
Hi guys,
I'm working on an exercise that uses the Turtlesim tool.
I have to create a node vel_filter that subscribes to messages published by the pubvel node (from the book A Gentle Introduction to Ros by O'Kane) and immediately republish only those messages who have positive angular velocity.
The pubvel code is given below:
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <stdlib.h>
int main(int argc, char**argv){
ros::init(argc, argv, "publish_velocity");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
srand(time(0));
ros::Rate rate(2);
while(ros::ok()){
geometry_msgs::Twist msg;
msg.linear.x = double(rand())/double(RAND_MAX);
msg.angular.z = 2*double(rand())/double(RAND_MAX) - 1;
pub.publish(msg);
ROS_INFO_STREAM("Sending random velocity command:"<<" linear="<<msg.linear.x<<" angular="<<msg.angular.z);
rate.sleep();
}
}
This publishes random linear and angular movements messages of type geometry/Twist on the turtle1/cmd_vel topic. Now I want to create a node vel_filter, subscribe on this topic to receive those messages, and publish only the messages with positive angular velocity on a different topic. (I'm not sure on which topic I should publish this).
What I have written so far for the vel_filter node is:
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>
void filterVelocityCallback(const geometry_msgs::Twist& msg){
if (msg.angular.z > 0){
ROS_INFO_STREAM("Subscriber velocities:"<<" linear="<<msg.linear.x<<" angular="<<msg.angular.z);
}
}
int main(int argc, char **argv){
ros::init(argc, argv, "filter_velocity");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("turtle1/cmd_vel",1000,&filterVelocityCallback);
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("turtle1/pose", 1000);
ros::Rate rate(2);
while(ros::ok()){
geometry_msgs::Twist msg;
pub.publish(msg);
ROS_INFO_STREAM("Filtered velocities:"<<" linear="<<msg.linear.x<<" angular="<<msg.angular.z);
rate.sleep();
}
ros::spin();
}
But this gives me the following:
[ INFO] [1492674948.750695494]: Filtered velocities: linear=0 angular=0
[ INFO] [1492674949.250756910]: Filtered velocities: linear=0 angular=0
[ INFO] [1492674949.750739740]: Filtered velocities: linear=0 angular=0
[ INFO] [1492674950.250739795]: Filtered velocities: linear=0 angular=0
[ INFO] [1492674950.750736997]: Filtered velocities: linear=0 angular=0
[ INFO] [1492674951.250766297]: Filtered velocities: linear=0 angular=0
Note that I didn't apply the filter yet. I used the turtle1/pose topic to publish (I have my doubts if this is correct). If I only apply the subscriber it works fine, but when I add the publisher it doesn't reach the callback function anymore.
My CMakeLists looks like this (don't bother about the vel_printer):
cmake_minimum_required(VERSION 2.8.3)
project(me41025_33)
find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs turtlesim)
catkin_package()
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(${PROJECT_NAME}_pubvel pubvel.cpp)
add_executable(${PROJECT_NAME}_vel_printer vel_printer.cpp)
add_executable(${PROJECT_NAME}_vel_filter vel_filter.cpp)
target_link_libraries(${PROJECT_NAME}_pubvel ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_vel_printer ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_vel_filter ${catkin_LIBRARIES})
set_target_properties(${PROJECT_NAME}_pubvel PROPERTIES OUTPUT_NAME pubvel PREFIX "")
set_target_properties(${PROJECT_NAME}_vel_printer PROPERTIES OUTPUT_NAME vel_printer PREFIX "")
set_target_properties(${PROJECT_NAME}_vel_filter PROPERTIES OUTPUT_NAME vel_filter PREFIX "")
And my package.xml looks like this:
<?xml version="1.0"?>
<package>
<name ...