Subscribing and publishing geometry/Twist messages from Turtlesim

edit

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 ...