unable to update footprint dynamically from c++

asked 2021-11-18 04:11:19 -0500

dinesh gravatar image

updated 2021-11-18 04:11:33 -0500

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.

edit retag flag offensive close merge delete

Comments

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.

Zonared gravatar image Zonared  ( 2023-06-01 06:24:50 -0500 )edit