Subscribing and publishing geometry/Twist messages

2019-09-14 23:42发布

问题:

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>me41025_33</name>
<version>0.0.0</version>
<description>The agitr package</description>

<maintainer email="stijn@todo.todo">stijn</maintainer>

<license>TODO</license>

    <build_depend>roscpp</build_depend>
    <run_depend>roscpp</run_depend>

<build_depend>geometry_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>
<build_depend>turtlesim</build_depend>
<run_depend>turtlesim</run_depend>
<buildtool_depend>catkin</buildtool_depend>

</package>

Thanks in advance for your help!

回答1:

You don't need ros::spin() here. You need ros::spinOnce(); inside the while loop. Read the documentation for explanation.

Also note that you called pubvel and vel_filter to be nodes in your post. These are not nodes. publish_velocity and filter_velocity respectively are the nodes here.

Furthermore, it would be better if you don't apply the conditions in the callback function and do that inside the while loop of the main function.

Sp your fixed vel_filter.cpp file will be this:

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>

float linx, angZ;

void filterVelocityCallback(const geometry_msgs::Twist& msg){
   //Using the callback function just for subscribing  
   //Subscribing the message and storing it in 'linx' and 'angZ'
   linx = msg.linear.x;      angZ = 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;
     msg.linear.x = linx;     msg.angular.z = angZ;

     //It would be better to apply the conditions within the main function and use the 
     //Callback function just for subscribing
     if (angZ > 0){
        ROS_INFO_STREAM("Subscriber velocities:"<<" linear="<<linx<<" angular="<<angZ);
        //The above line doesn't publish. It's like printf (but not exactly)
        pub.publish(msg);   //This line is for publishing. It publishes to 'turtle1/pose'
     }  
     rate.sleep();
     ros::spinOnce();      //Notice this
  } 
}

Recommended reading:

  1. Callbacks and Spinning.
  2. Writing a Publisher and a Subscriber.