Apriltag pkg - Node crash when trying to get position

asked 2018-11-16 05:20:27 -0600

michou214
1 ●2 ●3 ●3

updated 2018-11-21 10:17:55 -0600

Hello,

I'm trying to use a drone (iris model) with a camera in Gazebo to detect apriltag (AP). I cloned a usb-camera pkg, and the AP pkg. All works fine with the webcam. Now, I'm trying to use the position from the topic : /tag_detections_pose to get the position x,y,z. I'm coding in C++ with ROS Kinetic and use Gazebo 7.

I'm using the code provided (as base) by PX4 : https://dev.px4.io/en/ros/mavros_offboard.html

So I included the type of message: #include <geometry_msgs/PoseArray.h> // Tag detection Then, I create a global variable and a call back function:

   // Callback which will save the estimated local position of the Apriltag
   geometry_msgs::PoseArray APtag_est_pos; // Tag detection
   void APtag_est_pos_cb(const geometry_msgs::PoseArray::ConstPtr& AP_est_pos){
   APtag_est_pos = *AP_est_pos;
   ROS_INFO("CALLBACK AP");
   //    ROS_INFO("APtag est pos=[%f, %f, %f]",  AP_est_pos->poses[2].position.x,
   //                                            AP_est_pos->poses[2].position.y,
   //                                            AP_est_pos->poses[2].position.z);
   }

And finally I subscribe to the topic:

// The node subscribe to Topic "/tag_detections_pose", 10 msgs in buffer before deleting
ros::Subscriber APtag_est_pos_sub = nh.subscribe<geometry_msgs::PoseArray>
("tag_detections_pose", 10, APtag_est_pos_cb); // with or without the / ????

Like this it works, but if I try to uncomment the part that should give the position with ROS_INFO, I get this error :

[offb_node-5] process has died [pid 8044, exit code -11, cmd /home/mpperret/project_ws/devel/lib/waypoint_navigation/offb_node __name:=offb_node __log:=/home/mpperret/.ros/log/dbcc83d4-e8b8-11e8-acbf-54bf646662d7/offb_node-5.log].

Is someone know why ? Maybe I'm doing something wrong with the variable to get position x,y,z ?

Thanks in advance

edit retag flag offensive close merge delete

Comments

First thing to check: are there always 3 poses present in poses?

gvdhoorn ( 2018-11-16 05:22:21 -0600 )edit

You asked that because of the poses[2] (doc here)? I don't really know bacause I use the same writing for the topic /gazebo/ModelStates (here) and it works.

michou214 ( 2018-11-16 08:07:17 -0600 )edit

And if you compared both they had the same structure. For me I would use pose(s)[0] to get into position. If you have an explication, I would appreciate.

michou214 ( 2018-11-16 08:08:57 -0600 )edit

Yes, that's why I ask.

re: using it somewhere else: that is great, but is not how this should be approached. If AP_est_post->poses does not contain at least 3 elements, you are indexing into (potentially) uninitialised memory, which typically leads to crashes.

gvdhoorn ( 2018-11-16 08:08:59 -0600 )edit

And if you compared both they had the same structure. For me I would use pose(s)[0] to get into position. If you have an explication, I would appreciate.

Static structure of the messages is not really relevant, as I hope you (now) understand.

gvdhoorn ( 2018-11-16 08:09:49 -0600 )edit

So how could I save this information? What you said is true because when there is no AP, the topic message is "[-]" , and when there is one or more AP, the message is bigger. It's like a dynamical array with a defined structure. I don't know how to check if there is something or not.

michou214 ( 2018-11-16 08:15:31 -0600 )edit

That would be more of a basic C++ question, but poses is a std::vector, so you could check with methods such as size().

gvdhoorn ( 2018-11-16 08:46:05 -0600 )edit

I tried your suggestion and I get a value of 72. I'm pretty sure that is the memory size allocation for the variable. I'll do research on dynamical array or ask in a C++ forum. Thanks for your help. I will let you know if I found a solution.

michou214 ( 2018-11-16 12:03:03 -0600 )edit

see more comments

answered 2018-11-21 10:17:42 -0600

michou214
1 ●2 ●3 ●3

updated 2018-11-21 10:23:16 -0600

Hi,

I finally found a solution. It took me so long before finding these documentations so helpfull: PoseArray.h and geometry_msgs::PoseArray_< ContainerAllocator > Struct Template Reference

With that I discover some functions that I could use to solve my problem. I just made this modifications :

// Callback which will save the estimated local position of the Apriltag
//geometry_msgs::PoseArray APtag_est_pos;
void APtag_est_pos_cb(const geometry_msgs::PoseArray::ConstPtr& AP_est_pos){
    //ROS_INFO("CALLBACK AP");
    APtag_est_pos = *AP_est_pos;
    int n_AP = 0; // Or you can use it as a global variable
    n_AP = AP_est_pos->poses.size();
    ROS_INFO("n=%d", n_AP);
    if(n_AP>0){  
        ROS_INFO("APtag est pos=[%f, %f, %f]",  AP_est_pos->poses[0].position.x,
                                                AP_est_pos->poses[0].position.y,
                                                AP_est_pos->poses[0].position.z);
    }
}

Now it's working even with 2,3 AP detected. With the variable n_AP, you can switch between AP by using poses[n_AP-1] but don't forget to check there is one otherwise you will go in a wrong memory space.

I hope it could be helpful for others to not wasting time searching so simple thing...

Best regards.

edit flag offensive delete link

add a comment

Apriltag pkg - Node crash when trying to get position

Comments

1 Answer

Question Tools

Stats

Related questions

Apriltag pkg - Node crash when trying to get position edit

Comments

1 Answer

Question Tools

Stats

Related questions

Apriltag pkg - Node crash when trying to get position