[ROS Q&A] 045 – Publish and subscribe array of vector as message

In today’s Q&A, the question is How to use a c++ vector container in my code and to publish and subscribe it in ROS.

Let’s see how to do it.

Step 0. Create a project in ROS Development Studio(ROSDS)

ROSDS helps you follow our tutorial in a fast pace without dealing without setting up an environment locally. If you haven’t had an account yet, you can create a free account here. Let’s create a new project and call it ros_q_a_messages.

Step 1. Create a package

Let’s create a new package and a msg folder for our code.

cd ~/catkin_make/src
catkin_create_pkg my_pkg roscpp std_msgs geometry_msgs
cd my_pkg
mkdir msg

In the msg folder, we’ll define the message in the my_msg.msg file

geometry_msgs/Point[] points
uint8 another_field

In order to compile the message, we have to change two files. The first one is the pckage.xml, add the following two line.

...
<build_depend>message_generation</build_depend>
...
<run_depend>message_runtime</run_depend>
...

The second is the CMakeList.txt file. Change the following part

...
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  std_msgs
  message_generation
)
..
add_message_files(
  FILES
  my_msg.msg
)
...
generate_messages(
  DEPENDENCIES
  geometry_msgs
)
...
catkin_package(
  CATKIN_DEPENDS geometry_msgs roscpp std_msgs message_runtime
)
...

Then we can finally compile the message

cd ~/catkin_ws
catkin_make
source devel/setup.bash

In the my_pkg/src folder, create a my_publisher.cpp file with the following content

#include "ros/ros.h"
#include "geometry_msgs/Point.h"
#include "my_pkg/my_msg.h"

#include <vector>

struct Point {
    float x;
    float y;
};

int main(int argc, char **argv)
{
  // ROS objects
  ros::init(argc, argv, "my_publisher");
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<my_pkg::my_msg>("my_topic", 1);
  ros::Rate loop_rate(0.5);

  // the message to be published
  my_pkg::my_msg msg;
  msg.another_field = 0;

  // creating the vector
  Point my_array[11];
  Point point;
  for (int i=0; i < 11; i++) {
    point.x = i;
    point.y = i;
    my_array[i] = point;
  }
  std::vector<Point> my_vector (my_array, my_array + sizeof(my_array) / sizeof(Point));

  // loop control
  int count = 0;
  while (ros::ok())
  {
    msg.points.clear();

    msg.another_field = count;
    int i = 0;
    for (std::vector<Point>::iterator it = my_vector.begin(); it != my_vector.end(); ++it) {
        geometry_msgs::Point point;
        point.x = (*it).x;
        point.y = (*it).y;
        point.z = 0;
        msg.points.push_back(point);
        i++;
    }

    ROS_INFO("%d", msg.another_field);

    pub.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}

To compile the code, we have to change the CMakeLists.txt again in the following part.

...
add_executable(my_publisher src/my_publisher.cpp)
...
target_link_libraries(my_publisher
  $(catkin_LIBRARIES)
)

Then we compile it again with

cd ~/catkin_ws
catkin_make
source devel/setup.bash

Now you can run the executable rosrun my_pkg my_publisher with the node is publishing something.

You also have to run a simulation or run roscore to start the rosmaster

The next step is to create a subscriber to subscribe to the topic. Let’s create a my_subscriber.cpp file under the src folder with the following content.

#include "ros/ros.h"
#include "my_pkg/my_msg.h"

void clbk(const my_pkg::my_msg::ConstPtr& msg) {
    ROS_INFO("%d", msg->another_field);
    ROS_INFO("first point: x=%.2f, y=%.2f", msg->points[0].x, msg->points[0].y);
}

int main(int argc, char **argv)
{
  // ROS objects
  ros::init(argc, argv, "my_subscriber");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("my_topic", 1, clbk);

  ros::spin();

}

You also have to change the following part in CMakeLists.txt to compile the code

...
add_executable(my_subscriber src/my_subscriber.cpp)
...
target_link_libraries(my_subscriber $(catkin_LIBRARIES))

Then compile again and run with

cd catkin_ws
catlin_make
source devel/setup.bash
rosrun my_pkg my_subscriber

You should see the number from the my_topic is printing on the screen.

Edit by: Tony Huang