How to subscribe to Array message
I'm writing a program that subscribes to the topic "color / yolov4_Spatial_detections", gets the xyz coordinate value from "geometry_msgs / Point position", gets it with MOVEIT, and does pick and place work.
For now, I'm creating a simple node that subscribes to xyz from "yolov4_Spatial_detections".
I don't know in detail how to subscribe to a message file with an Array structure, and the following error occurs and the subscribe node goes down.
I have confirmed that [color / yolov4_Spatial_detections] is published in the rostopic list. I also checked the xyz coordinate information with a rostopic echo.
Can you give me some advice on how to subscribe to message files with this structure?
Thank you
#include <ros/ros.h>
#include "geometry_msgs/Point.h"
#include <depthai_ros_msgs/SpatialDetection.h>
#include <depthai_ros_msgs/SpatialDetectionArray.h>
//void clbk(const depthai_ros_msgs::SpatialDetection::ConstPtr& msg) {
void clbk(const depthai_ros_msgs::SpatialDetectionArray::ConstPtr& msg) {
ROS_INFO("position: x=%.2f, y=%.2f", msg->detections[0].position.x, msg->detections[0].position.y);
}
int main(int argc, char **argv)
{
// ROS objects
ros::init(argc, argv, "yolov4_subscriber_node");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/yolov4_publisher/color/yolov4_Spatial_detections", 1, clbk);
ros::spin();
}
error
process[yolov4_subscriber-1]: started with pid [8158]
================================================================================REQUIRED process [yolov4_subscriber-1] has died!
process has died [pid 8158, exit code -11, cmd /home/yo4hi6o/oak_ws/devel/lib/depthai_examples/yolov4_subscriber_node __name:=yolov4_subscriber __log:=/home/yo4hi6o/.ros/log/37964ede-bd18-11ec-a47c-e094679e2019/yolov4_subscriber-1.log].
log file: /home/yo4hi6o/.ros/log/37964ede-bd18-11ec-a47c-e094679e2019/yolov4_subscriber-1*.log
Initiating shutdown!
================================================================================
[yolov4_subscriber-1] killing on exit
depthai_ros_msgs/SpatialDetectionArray.msg
# A list of 2D detections, for a multi-object 2D detector along with 3D depth information.
std_msgs/Header header
# A list of the detected proposals. A multi-proposal detector might generate along with the 3D depth information
# this list with many candidate detections generated from a single input.
SpatialDetection[] detections
## These messages are created as close as possible to the official vision_msgs(http://wiki.ros.org/vision_msgs)
depthai_ros_msgs/SpatialDetection,msg
# Class probabilities
vision_msgs/ObjectHypothesis[] results
# 2D bounding box surrounding the object.
vision_msgs/BoundingBox2D bbox
# Center of the detected object in meters
geometry_msgs/Point position
# If true, this message contains object tracking information.
bool is_tracking
# ID used for consistency across multiple detection messages. This value will
# likely differ from the id field set in each individual ObjectHypothesis.
# If you set this field, be sure to also set is_tracking to True.
string tracking_id
rostopic echo /yolov4_publisher/color/yolov4_Spatial_detections
detections:
-
results:
-
id: 0
score: 0.507520675659
bbox:
center:
x: 238.0
y: 242.0
theta: 0.0
size_x: 388.0
size_y: 374.0
position:
x: 0.0630802065134
y: -0.0779226049781
z: 1.63851070404
is_tracking: False
tracking_id: ''
---
header:
seq: 221
stamp:
secs: 1650080057
nsecs: 312681360
frame_id: "oak_rgb_camera_optical_frame"
Message with an array or not makes no difference.
Before calling
msg->detections[0]
, what do you get if you logmsg->detections.size()
?@abhishek47 thankyou msg-> detections.size () If you get the log of, the value of 1 is returned when the object is detected.
@abhishek47 Thankyou. I didn't know how to subscribe to an array message file. Based on your tips I was able to solve it with reference to this issue. Thank you.