Hello everyone, I am a doctoral candidate in the direction of SLAM. I know the ups and downs of the SLAM learning process along the way, and I am very grateful to everyone for their high-quality articles and source code. With more and more knowledge, I am going to organize a column of my own laser SLAM study notes, from 0 to take everyone to quickly get started with laser SLAM, and it is also convenient for students who want to get started with SLAM and Xiaobai study reference, I believe you can read it There will be certain gains in the end. If there is something wrong, please point it out, and welcome everyone to exchange and discuss and make progress together.
Table of contents
An important point of ROS is that it can receive and send different types of messages, and can visualize data in real time. ROS official website has given many message types for us to use.
1. std_msgs
1.1 Introduction
std_msgs is a standard message type package that contains message definitions for some commonly used basic data types.
1.2 Basic categories
std_msgs/Bool: Represents a Boolean value (True or False)
std_msgs/Int8, Int16, Int32, Int64: represent signed 8, 16, 32, and 64-bit integers
std_msgs/UInt8, UInt16, UInt32, UInt64: represent unsigned 8, 16, 32, and 64-bit integers
std_msgs/Float32, Float64: represent single-precision and double-precision floating-point numbers
std_msgs/String: represents a string
std_msgs also includes other types of messages, such as:
std_msgs/Time: indicates the timestamp of ROS
std_msgs/Duration: Indicates the time period
std_msgs/Header: Indicates the ROS message header, which includes information such as timestamp, coordinate system and serial number.
1.3 Using Templates
post receive string
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
void callback_string(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("Received string: %s", msg->data.c_str());
// 创建一个新的std_msgs::String类型消息
std_msgs::String output_msg;
output_msg.data = "Received string: " + msg->data;
// 发布新消息
pub.publish(output_msg);
}
void callback_int(const std_msgs::Int32::ConstPtr& msg)
{
ROS_INFO("Received integer: %d", msg->data);
// 创建一个新的std_msgs::String类型消息
std_msgs::String output_msg;
output_msg.data = "Received integer: " + std::to_string(msg->data);
// 发布新消息
pub.publish(output_msg);
}
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "listener_and_talker");
// 创建ROS节点句柄和两个订阅节点对象，以及一个发布者对象
ros::NodeHandle nh;
ros::Subscriber sub_string = nh.subscribe("my_topic_string", 10, callback_string);
ros::Subscriber sub_int = nh.subscribe("my_topic_int", 10, callback_int);
ros::Publisher pub = nh.advertise<std_msgs::String>("my_topic_output", 10);
// 运行ROS节点
ros::spin();
return 0;
}Two, geometry_msgs
2.1 Introduction
geometry_msgs in ROS is a package that contains various geometry-related message types, mainly used in robot motion control and perception-related applications.
2.2 Basic categories
- Point: used to represent a point in three-dimensional space.
- Quaternion: used to represent a quaternion, usually used to represent the pose of a robot end effector.
- Pose: It is used to represent a pose in three-dimensional space, that is, position and attitude.
- Vector3: used to represent a vector in three-dimensional space.
- Twist: used to represent the linear velocity and angular velocity of the robot in three-dimensional space.
- Transform: It is used to represent the transformation relationship between two coordinate systems, that is, the translation and rotation of one coordinate system relative to another coordinate system.
- PoseStamped: Used to represent poses in 3D space with timestamps.
- TwistStamped: used to represent the linear velocity and angular velocity of the robot in three-dimensional space with time stamps.
- TransformStamped: Used to represent the transformation relationship between two coordinate systems with timestamps.
- Accel: Used to indicate the linear acceleration and angular acceleration of the robot in three-dimensional space.
- AccelStamped: Used to represent the linear acceleration and angular acceleration of the robot in three-dimensional space with time stamps.
For more usage methods, see the official website https://wiki.ros.org/geometry_msgs?distro=melodic
2.3 Using Templates
发布接收位置
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
// 在回调函数中处理接收到的消息
ROS_INFO("Received a pose at time %f", msg->header.stamp.toSec());
ROS_INFO("Position: x=%f, y=%f, z=%f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
ROS_INFO("Orientation: x=%f, y=%f, z=%f, w=%f", msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pose_node"); // 初始化ROS节点
ros::NodeHandle nh;
// 创建一个用于接收pose消息的订阅者
ros::Subscriber sub = nh.subscribe<geometry_msgs::PoseStamped>("pose_topic", 10, poseCallback);
// 创建一个用于发送pose消息的发布者
ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("pose_topic", 10);
// 设置消息的头部信息
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.stamp = ros::Time::now();
pose_msg.header.frame_id = "map";
// 设置消息的内容
pose_msg.pose.position.x = 1.0;
pose_msg.pose.position.y = 2.0;
pose_msg.pose.position.z = 3.0;
pose_msg.pose.orientation.x = 0.0;
pose_msg.pose.orientation.y = 0.0;
pose_msg.pose.orientation.z = 0.0;
pose_msg.pose.orientation.w = 1.0;
// 发布消息
pub.publish(pose_msg);
ros::spin();
return 0;
}
Three, sensor_msgs
3.1 Introduction
sensor_msgs in ROS is a message type used to transfer sensor data related messages between ROS nodes. sensor_msgs contains message types for many commonly used sensors, such as lidar, camera, IMU, GPS, etc.
3.2 Basic categories
- Imu: information such as attitude, angular velocity and linear acceleration.
- MagneticField: Magnetic field strength data.
- NavSatFix: GPS positioning data.
- NavSatStatus: GPS positioning status data.
- Joy: handle controller data.
- JoyFeedback: joystick feedback data.
- JoyFeedbackArray: joystick feedback array data.
- LaserScan: LiDAR data.
- PointCloud: point cloud data.
- PointCloud2: point cloud data, supports RGB information.
- CameraInfo: Camera internal reference data.
- Image: image data.
- CompressedImage: Compressed image data.
- FluidPressure: hydraulic pressure data.
- TFMessage: Coordinate system transformation data.
- NavSatFix: GPS data.
- TimeReference: time reference data.
- MultiArrayDimension: dimension information of multidimensional array data.
- MultiArrayLayout: layout information of multidimensional array data.
- ImageProjection: Image projection data for 3D reconstruction.
- PointField: field information of point cloud data.
- RegionOfInterest: Region of interest data.
- RelativePose: Relative pose data.
- RelativePoseStamped: Relative pose and timestamp data.
- NavSatCartesian: Convert GPS coordinates to Cartesian coordinate data.
- CompressedNavSatFix: Compressed GPS data.
3.3 Using Templates
Publish Receive Point Cloud
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& input_cloud_msg)
{
// TODO: 这里添加处理输入点云的代码
// 将处理后的点云发布到 /output_pointcloud 话题上
pub.publish(output_cloud_msg);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "point_cloud_node");
ros::NodeHandle nh;
// 订阅 /input_pointcloud 话题上的点云消息
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/input_pointcloud", 1, pointCloudCallback);
// 发布处理后的点云消息到 /output_pointcloud 话题上
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/output_pointcloud", 1);
ros::spin();
return 0;
}Four, shape_msgs
4.1 Introduction
shape_msgs in ROS is a ROS message type that defines common geometric shapes (such as points, lines, polygons, cubes, etc.). It is one of the standard messages of ROS and is often used to describe the shape and posture of the robot, as well as obstacles in the environment. The shape_msgs message cannot be directly visualized in rviz, it needs to be converted into a visualization_msgs/Marker message to be displayed in rviz. The visualization_msgs/Marker message can be visualized directly in rviz, and provides a variety of different visualization methods, such as displaying geometric shapes with lines, surfaces, and arrows.
4.2 Basic categories
- Point: A point in a three-dimensional space, represented by three floating-point numbers x, y, and z.
- Quaternion: A quaternion represented by four floating-point numbers x, y, z, and w.
- Vector3: A three-dimensional vector represented by three floating-point numbers x, y, and z.
- Pose: A pose, consisting of a position (Point) and a direction (Quaternion).
- PoseStamped: Pose with timestamp.
- Twist: The linear velocity and angular velocity of a rigid body, composed of linear velocity (Vector3) and angular velocity (Vector3).
- Polygon: A polygon in a plane, composed of multiple points (Point).
- Mesh: A three-dimensional model composed of multiple triangles (Triangle).
- SolidPrimitive: The basic geometric shape of a solid, such as a cube, sphere, cylinder, etc.
- MeshTriangle: A triangle consisting of indices of three vertices used to describe a triangle in a Mesh message.
4.3 Using templates
Send triangle vertex coordinates
#include <ros/ros.h>
#include <shape_msgs/Polygon.h>
#include <geometry_msgs/Point32.h>
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "polygon_publisher");
// 创建节点句柄
ros::NodeHandle nh;
// 创建一个publisher发布名为"/polygon"的shape_msgs/Polygon消息
ros::Publisher polygon_pub = nh.advertise<shape_msgs::Polygon>("/polygon", 10);
// 设置消息发布频率
ros::Rate loop_rate(10);
while (ros::ok())
{
// 创建shape_msgs/Polygon消息并填充数据
shape_msgs::Polygon polygon;
geometry_msgs::Point32 p1, p2, p3;
p1.x = 0.0;
p1.y = 0.0;
p1.z = 0.0;
p2.x = 1.0;
p2.y = 0.0;
p2.z = 0.0;
p3.x = 0.0;
p3.y = 1.0;
p3.z = 0.0;
polygon.points.push_back(p1);
polygon.points.push_back(p2);
polygon.points.push_back(p3);
// 发布消息
polygon_pub.publish(polygon);
// 按照指定频率循环发布消息
loop_rate.sleep();
}
return 0;
}Receive triangle vertex coordinates
#include <ros/ros.h>
#include <shape_msgs/Polygon.h>
#include <geometry_msgs/Point32.h>
void polygonCallback(const shape_msgs::Polygon::ConstPtr& msg)
{
// 输出消息内容
ROS_INFO("Received polygon message:");
for (int i = 0; i < msg->points.size(); i++)
{
ROS_INFO("Point %d: (%f, %f, %f)", i+1, msg->points[i].x, msg->points[i].y, msg->points[i].z);
}
}
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "polygon_subscriber");
// 创建节点句柄
ros::NodeHandle nh;
// 创建一个subscriber订阅名为"/polygon"的shape_msgs/Polygon消息
ros::Subscriber polygon_sub = nh.subscribe("/polygon", 10, polygonCallback);
// 循环处理回调函数
ros::spin();
return 0;
}5. trajectory_msgs
5.1 Introduction
trajectory_msgs is a ROS message package containing message types used to describe the motion of a robot or other object. It mainly includes the following message types:
5.2 Basic types
- JointTrajectory: Describes the trajectory of the robot's joint motion, including information such as joint position, velocity, acceleration, and timestamp.
- JointTrajectoryPoint: Describes the joint state of the robot at a moment, including joint position, velocity, acceleration and other information.
- MultiDOFJointTrajectory: Describes the multi-degree-of-freedom motion trajectory of the robot, including the position, velocity, acceleration, time stamp and other information of multiple joints.
- MultiDOFJointTrajectoryPoint: Describes the multi-degree-of-freedom state of the robot at a moment, including information such as the position, velocity, and acceleration of multiple joints.
5.3 Basic types
The publisher periodically creates a JointTrajectory message and publishes it to the topic joint_trajectory, and the subscriber receives the message through the callback function jointTrajectoryCallback and prints out the joint information in it.
#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "joint_trajectory_publisher_subscriber");
ros::NodeHandle nh;
// 创建一个发布者，用于发布机器人的关节轨迹
ros::Publisher joint_trajectory_pub = nh.advertise<trajectory_msgs::JointTrajectory>("joint_trajectory", 10);
// 创建一个订阅者，用于接收机器人的关节轨迹
ros::Subscriber joint_trajectory_sub = nh.subscribe("joint_trajectory", 10, jointTrajectoryCallback);
ros::Rate loop_rate(10); // 发布频率为10Hz
while (ros::ok())
{
// 创建一个JointTrajectory消息
trajectory_msgs::JointTrajectory trajectory_msg;
trajectory_msg.header.stamp = ros::Time::now();
trajectory_msg.joint_names.push_back("joint_1");
trajectory_msg.joint_names.push_back("joint_2");
trajectory_msg.joint_names.push_back("joint_3");
// 创建一个JointTrajectoryPoint消息
trajectory_msgs::JointTrajectoryPoint point_msg;
point_msg.positions.push_back(0.5);
point_msg.positions.push_back(0.5);
point_msg.positions.push_back(0.5);
point_msg.velocities.push_back(0.0);
point_msg.velocities.push_back(0.0);
point_msg.velocities.push_back(0.0);
point_msg.time_from_start = ros::Duration(1.0); // 在1秒的时间内到达目标位置
trajectory_msg.points.push_back(point_msg); // 将JointTrajectoryPoint添加到JointTrajectory中
// 发布JointTrajectory消息
joint_trajectory_pub.publish(trajectory_msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
// 订阅者回调函数，用于接收机器人的关节轨迹
void jointTrajectoryCallback(const trajectory_msgs::JointTrajectory& msg)
{
ROS_INFO("Received JointTrajectory message");
// 打印出轨迹中的关节信息
for (int i = 0; i < msg.joint_names.size(); i++)
{
ROS_INFO("%s position: %f, velocity: %f, acceleration: %f",
msg.joint_names[i].c_str(),
msg.points[0].positions[i],
msg.points[0].velocities[i],
msg.points[0].accelerations[i]);
}
}Six, nav_msgs
6.1 Introduction
nav_msgs in ROS is a set of packages related to navigation related messages (messages) and service (services) types. It contains some standard message formats for navigation tasks, such as the robot's current pose information, target points, obstacles and other information.
6.2 Basic types
- Odometry (odometer message): Provide information such as the current pose and speed of the robot.
- Path (path message): Provides the path information of the robot, which can be used to visualize the trajectory of the robot on the map.
- MapMetaData (map metadata message): Provides basic information about the map, such as resolution, start point and end point of the map, etc.
- OccupancyGrid (occupancy grid map message): Provides information about the environment map around the robot, including obstacles and free space.
- GetMap (get map service): used to get the occupancy grid map from the map server.
6.3 Using templates
Publish to receive a Path, which can be visualized directly in RVIZ
#include <ros/ros.h>
#include <nav_msgs/Path.h>
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "path_publisher");
ros::NodeHandle nh;
// 创建一个发布者，发布类型为nav_msgs::Path，话题名为/path
ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("path", 10);
// 创建一个订阅者，订阅类型为nav_msgs::Path，话题名为/path
ros::Subscriber path_sub = nh.subscribe<nav_msgs::Path>("path", 10, pathCallback);
// 循环发布消息
ros::Rate loop_rate(10);
while (ros::ok())
{
// 创建一个新的Path消息
nav_msgs::Path path_msg;
// 将路径信息添加到Path消息中，此处省略具体代码
...
// 发布Path消息
path_pub.publish(path_msg);
// 循环等待
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
// 接收到Path消息时的回调函数
void pathCallback(const nav_msgs::Path::ConstPtr& path_msg)
{
// 处理接收到的Path消息，此处省略具体代码
...
}七、visualization_msgs
7.1 Introduction
visualization_msgs is a message type package for visualization in ROS. It contains some standard message formats for visualization in ROS, such as points, lines, arrows, grids, text, markers, etc.
7.2 Basic types
- Marker: Used to visualize points, lines, arrows, grids, text, etc. as objects in 3D space.
- MarkerArray (marker array message): used to publish multiple Marker messages at the same time.
- ImageMarker (image marker message): Used to visualize an image as an object in 3D space.
- InteractiveMarker (interactive marker message): Used to add visual elements that can interact with the user, such as moving, rotating, scaling, etc.
https://wiki.ros.org/visualization_msgs?distro=melodic
7.3 Marker uses templates
*发布接收
发布Marker消息
ros::Publisher vis_pub = nh.advertise<visualization_msgs::Marker>( "visualization_marker", 0, markerCallback);
接收Marker消息
ros::Subscriber sub = nh.subscribe<visualization_msgs::Marker>("marker_topic", 10, markerCallback);
*消息内容
//基本信息
visualization_msgs::Marker marker;
marker.header.frame_id = "base_link";
marker.header.stamp = ros::Time();
marker.ns = "my_namespace";
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE;//确定类型
marker.action = visualization_msgs::Marker::ADD;
//位置信息
marker.pose.position.x = 1;
marker.pose.position.y = 1;
marker.pose.position.z = 1;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
//Marker范围
marker.scale.x = 1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
//Marker颜色
marker.color.a = 1.0; // Don't forget to set the alpha!
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
//加载地图
marker.mesh_resource="package://pr2_description/meshes/base_v0/base.dae";
//发布消息
vis_pub.publish( marker );
build an arrow
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
int main( int argc, char** argv )
{
ros::init(argc, argv, "basic_shapes");
ros::NodeHandle n;
ros::Rate r(1);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
while (ros::ok())
{
visualization_msgs::Marker marker;
marker.header.frame_id = "base_link";
marker.header.stamp = ros::Time::now();
marker.ns = "basic_shapes";
marker.id = 0;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = 1.0;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.r = 1.0f;
marker.color.g = 0.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker_pub.publish(marker);
r.sleep();
}
}
Create a cube list
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
int main( int argc, char** argv )
{
ros::init(argc, argv, "basic_shapes");
ros::NodeHandle n;
ros::Rate r(1);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
visualization_msgs::Marker marker;
marker.header.frame_id = "base_link";
marker.header.stamp = ros::Time::now();
marker.ns = "basic_shapes";
marker.type = visualization_msgs::Marker::CUBE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0;
for (int i = 0; i < 10; i++)
{
for (int j = 0; j < 10; j++)
{
for (int k = 0; k < 10; k++)
{
geometry_msgs::Point cube_point;
cube_point.x = i * 0.1;
cube_point.y = j * 0.1;
cube_point.z = k * 0.1;
marker.points.push_back(cube_point);
}
}
}
while (ros::ok())
{
marker_pub.publish(marker);
r.sleep();
}
}
Eight, jsk_recognition_msgs
8.1 Introduction
jsk_recognition_msgs is a ROS message package that contains some common message types for robot vision and perception. These message types can be used to communicate robot perception and recognition results in the ROS system.
8.2 Categories
It mainly includes the following message types:
- BoundingBox: A 3D bounding box used to represent an object, including information such as its size and position.
- BoundingBoxArray: An array composed of multiple BoundingBoxes, used to represent the bounding boxes of multiple objects.
- ClusterPointIndices: used to represent the cluster information in the point cloud, including the index of each cluster point.
- ContactSensor: Used to represent measurements from robot contact sensors, such as touch sensors and force sensors.
- Histogram: used to represent the histogram information of an image or point cloud.
- LabelArray: An array of labels representing the label for each pixel or point in the image or point cloud.
- ModelCoefficientsArray: An array composed of multiple model coefficients, used to represent the coefficients of multiple geometric models.
- PolygonArray: An array composed of multiple polygons, used to represent the surface geometric information of multiple objects.
- RectArray: An array composed of multiple rectangles, used to represent information such as the position and size of multiple image regions.
- Segment: Used to represent a segmentation result in the point cloud, including information such as the segmentation label and coefficient to which each point belongs.
- Segmentation: An array composed of multiple Segments, used to represent multiple segmentation results in the point cloud.
- TrackingObject: Used to represent information about a tracked object, including its identifier, position, and velocity.
- The jsk_recognition_msgs package provides some convenient message types that can be used to process robot perception and recognition data.
8.3 Using Templates
自定义发送
#include <ros/ros.h>
#include <jsk_recognition_msgs/BoundingBoxArray.h>
#include <jsk_recognition_msgs/BoundingBox.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "custom_bounding_box_array_publisher");
ros::NodeHandle nh;
// 创建一个 BoundingBoxArray 发布者
ros::Publisher bb_array_pub = nh.advertise<jsk_recognition_msgs::BoundingBoxArray>("bounding_box_array_topic", 10);
// 创建一个 BoundingBoxArray 消息
jsk_recognition_msgs::BoundingBoxArray bb_array_msg;
// 设置 BoundingBoxArray 消息的 Header
bb_array_msg.header.frame_id = "base_link";
bb_array_msg.header.stamp = ros::Time::now();
// 创建一个 BoundingBox 消息并添加到 BoundingBoxArray 消息中
jsk_recognition_msgs::BoundingBox bb_msg;
bb_msg.header = bb_array_msg.header;
bb_msg.label = "object_1"; // 设置标签
bb_msg.pose.position.x = 1.0; // 设置位置坐标
bb_msg.pose.position.y = 2.0;
bb_msg.pose.position.z = 3.0;
bb_msg.dimensions.x = 0.5; // 设置尺寸
bb_msg.dimensions.y = 0.3;
bb_msg.dimensions.z = 0.2;
bb_array_msg.boxes.push_back(bb_msg); // 将 BoundingBox 消息添加到 BoundingBoxArray 中
// 发布 BoundingBoxArray 消息
bb_array_pub.publish(bb_array_msg);
ROS_INFO("Custom BoundingBoxArray published");
ros::spin();
return 0;
}
接收获取坐标
#include <ros/ros.h>
#include <jsk_recognition_msgs/BoundingBoxArray.h>
void boundingBoxCallback(const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg) {
// 遍历BoundingBoxArray中的所有BoundingBox
for (const auto& bbox : msg->boxes) {
// 获取BoundingBox的尺寸
const auto& dimensions = bbox.dimensions;
float width = dimensions.x;
float height = dimensions.y;
float depth = dimensions.z;
// 获取BoundingBox的坐标
const auto& position = bbox.pose.position;
float x = position.x;
float y = position.y;
float z = position.z;
// 在控制台输出BoundingBox的尺寸和坐标
ROS_INFO_STREAM("Bounding Box Dimensions (width, height, depth): " << width << ", " << height << ", " << depth);
ROS_INFO_STREAM("Bounding Box Position (x, y, z): " << x << ", " << y << ", " << z);
}
}
int main(int argc, char** argv) {
// 初始化ROS节点
ros::init(argc, argv, "bounding_box_subscriber");
// 创建节点句柄
ros::NodeHandle nh;
// 订阅jsk_recognition_msgs::BoundingBoxArray消息
ros::Subscriber sub = nh.subscribe<jsk_recognition_msgs::BoundingBoxArray>("bounding_box_topic", 1000, boundingBoxCallback);
// 循环等待回调函数
ros::spin();
return 0;
}
In the above code, we define a callback function boundingBoxCallback() to process the received jsk_recognition_msgs::BoundingBoxArray message. In this callback function, we iterate over all BoundingBoxes in BoundingBoxArray, and get its size and coordinate information from each BoundingBox. Specifically, we use bbox.dimensions to get the size of the BoundingBox, and bbox.pose.position to get the position of the BoundingBox. Finally, we output the dimensions and coordinates of each BoundingBox to the console so we can inspect them while the program is running.
At this point, the ROS explanation part of the column is over. You should know what ROS can do and write simple codes for sending and receiving messages. Next, we will introduce the theoretical basis of SLAM.