unable to update footprint dynamically from c++
I am trying to update the footprint of the robot dynamically from my ros c++ node. The data type of footprint is goemetry_msgs/Polygon. Here is my code which is not working:
std::string footprint_str = "-0.25,-0.25,-0.25,0.25,0.37,0.25,0.37,-0.25";
// Updating the robot footprint
for (int i = 0; i<footprint->polygon.points.size(); i++)
{
geometry_msgs::Point pnt;
pnt.x = footprint->polygon.points[i].x;
pnt.y = footprint->polygon.points[i].y;
pnt.z = footprint->polygon.points[i].z;
tf2::doTransform(pnt, pnt, transformStamped); // Uncomment to transform robot footprint acording to body_link rotation
}
std::cout<<footprint_str<<std::endl;
robot_footprint.name = "footprint";
robot_footprint.value = footprint_str;
conf.strs.push_back(robot_footprint);
srv_req.config = conf;
if (ros::service::call(footprint_srv, srv_req, srv_resp)) {
ROS_INFO("footprint parameters updated");
} else {
ROS_INFO("unable to update footprint parameters");
}
When i do the same from the rqt reconfigure gui it works like charm.
Hi there, I don't know if you solved this or not but I found a workaround here. If you did solve it, what did you end up doing?
Also I would be interesting in your polygon array to tf2::doTransfrom solution if you have this working, thanks.