Ask Your Question
0

Subscribing and publishing geometry/Twist messages from Turtlesim

asked 2017-04-20 03:24:52 -0500

stijndelo gravatar image

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

2 Answers

Sort by » oldest newest most voted
0

answered 2017-04-20 21:27:55 -0500

Geoff gravatar image

There are two problems in your code for vel_filter.

The first is that the publisher is always going to publish twists with all values at zero. There is no connection between the msg variable in your while loop and the msg variable that the subscriber callback receives. If you want to publish the filtered velocities, you should pass the publisher to the subscriber callback as an argument and do the publishing in the callback.

The second is that you go into a while loop that will never end until the node shuts down, but you do not provide any time for the ROS infrastructure inside that loop. ros::spin() will only be called when the loop exits, at which point the node is shutting down and it will return instantly. In order to use a while loop with a timed sleep like you are doing here, you need to call ros::spinOnce() after your sleep. This will provide the ROS infrastructure with one cycle of processing its buffers, subscriptions, etc., and allow the filterVelocityCallback() to be called.

edit flag offensive delete link more
0

answered 2017-04-25 04:57:19 -0500

bobpop gravatar image

Hi,friends,I see your code. In the first,I think that you should use rostopic listconfirm that your publisher is right or not.I describe that you just got the result of the "ROS_Info".Add the .msg files in the cmakelists and the msg file,use the rostopic to check publisher and subscriber. Second,your filter is in the void callback,you can't get the filtered data if you haven't published from void callback. That's all. Good luck!

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2017-04-20 03:24:52 -0500

Seen: 4,885 times

Last updated: Apr 25 '17