mismatching topics ar_track_alvar_msgs
Greetings~
While trying to reach position/orientation of detected marker, getting this kind of error:
[ERROR] [1544117434.771301999]: Client [/vision_node] wants topic /ar_pose_marker to have datatype/md5sum [ar_track_alvar_msgs/AlvarMarker/ef2b6ad42bcb18e16b22fefb5c0fb85f], but our version has [ar_track_alvar_msgs/AlvarMarkers/943fe17bfb0b4ea7890368d0b25ad0ad]. Dropping connection.
I do understand an issue, which is I'm using another structure of msg in vision_node, than ar_track_alvar advertiser.
In this case, how could I get a header on 'pose' of marker detected.
Note: I believe, ar_track_alvar_msgs/AlvarMarkers is done to detect a bunch of markers, but not only one. I only need to detect a single, so it doesn't matter for me, which kind of message to use.
Attaching part of /vision_node code down below:
Localizer(ros::NodeHandle& nh)
{
ar_sub_ = nh.subscribe<ar_track_alvar_msgs::AlvarMarker>("ar_pose_marker", 1,
&Localizer::visionCallback, this);
server_ = nh.advertiseService("localize_part", &Localizer::localizePart, this);
}
void visionCallback(const ar_track_alvar_msgs::AlvarMarkerConstPtr& msg)
{
last_msg_ = msg;
//ROS_INFO_STREAM(last_msg_->pose.pose);
}
bool localizePart(myworkcell_core::LocalizePart::Request& req,
myworkcell_core::LocalizePart::Response& res)
{
// Read last message
ar_track_alvar_msgs::AlvarMarkerConstPtr p = last_msg_;
if (!p) return false;
tf::Transform cam_to_target;
tf::poseMsgToTF(p->pose.pose, cam_to_target);
tf::StampedTransform req_to_cam;
listener_.lookupTransform(req.base_frame, p->header.frame_id, ros::Time(0), req_to_cam);
tf::Transform req_to_target;
req_to_target = req_to_cam * cam_to_target;
tf::poseTFToMsg(req_to_target, res.pose);
return true;
}
ros::Subscriber ar_sub_;
ar_track_alvar_msgs::AlvarMarkerConstPtr last_msg_;
ros::ServiceServer server_;
tf::TransformListener listener_;
Feel free to clarify, I am bad at explaining:D
[EDIT]:
LocalizePart service:
#request
string base_frame
---
#response
geometry_msgs/Pose pose
[EDIT2]:
Found solution after checking ar_track_alvar messages available and only got this line changed:
tf::poseMsgToTF(p->markers[0].pose.pose, cam_to_target);
Code compiled, my stupid mistake. It's not about ROS versions or package conflicts, but because there are two types of msg offered by package and I checked a wrong one.
Anyways might be helpful to other beginners to go through rosmsg structure.