c++ - 从 Turtlesim 订阅和发布几何/扭曲消息

Question

我正在做一个使用 Turtlesim 工具的练习。

我必须创建一个节点 vel_filter 来订阅 pubvel 节点发布的消息(来自 O'Kane 的《对 Ros 的简要介绍》一书)，并立即仅重新发布那些具有正角速度的消息。

发布代码如下:

#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();
}
}

这会在 turtle1/cmd_vel 主题上发布类型为 geometry/Twist 的随机线性和角运动消息。现在我想创建一个节点 vel_filter，订阅该主题以接收这些消息，并仅发布在不同主题上具有正角速度的消息。 (我不确定我应该在哪个主题上发布这篇文章)。

到目前为止，我为 vel_filter 节点编写的是:

#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();
}

但这给了我以下信息:

[ 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

请注意，我还没有应用过滤器。我使用 turtle1/pose 主题发布(我怀疑这是否正确)。如果我只应用订阅者，它工作正常，但当我添加发布者时，它不再到达回调函数。

我的 CMakeLists 看起来像这样(不用担心 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 "")

我的 package.xml 看起来像这样:

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

预先感谢您的帮助!

Answer 1

这里不需要ros::spin()。您需要在 while 循环中使用 ros::spinOnce();。阅读 documentation进行解释。

另请注意，您将 pubvel 和 vel_filter 称为帖子中的节点。这些不是节点。 publish_velocity和filter_velocity分别是这里的节点。

此外，如果您不在回调函数中应用条件，而是在主函数的 while 循环中应用条件会更好。

Sp 你的固定 vel_filter.cpp 文件将是这样的:

#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
  } 
}

---

推荐阅读:

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