what is the correct way to change dynamic footprint?

asked 2023-05-06 01:32:09 -0500

Zonared
48 ●5 ●6 ●12

Hello everyone, I am trying to change the shape of my polygon footprint dynamically from a cpp node and cant seem to find the correct way or a working example.

Some quick background, I have a center/centre steering articulated robot, which means the footprint changes with respect to articulation angle. I'm using TEB nav and move_base, TEB nav supports dynamic footprint but I cant find examples of how to use it.

If I use rqt_reconfigure (i.e. dynamic reconfigure) I can adjust the footprint string and can see it change in rviz. This is my footrpint:

 base_local_planner_params.yaml
   footprint_model:
     type: "polygon"
     vertices: [[0.681,-0.280],[0.356,-0.280],[0.166,-0.300],[-0.159,-0.300],[-0.166,0.300],[0.166,0.300],[0.356,0.280],[0.681,0.280]]

It appears you cant publishing to topic /move_base/local_costmap/footprint because its frame_id is odom, you also cant publish to move_base/global_costmap/footprint because its frame_id is map, you need to change the footprint of the robot, probably base_link frame or something.

I then tried to change the parameter footprint by writing to /move_base/local_costmap/footprint using rosparam. First if I use this command to read/get current parameter:

$rosparam get /move_base/local_costmap/footprint
  '[[0.681,-0.28],[0.356,-0.28],[0.166,-0.3],[-0.159,-0.3],[-0.166,0.3],[0.166,0.3],[0.356,0.28],[0.681,0.28]]'

Then try to change the first point to [1.0,-0.28] using this command, I end up with a different looking string and the footprint doesn't change:

$rosparam set /move_base/local_costmap/footprint '[[1.0,-0.28],[0.356,-0.28],[0.166,-0.3],[-0.159,-0.3],[-0.166,0.3],[0.166,0.3],[0.356,0.28],[0.681,0.28]]'
   - - 1.0
     - -0.28
   - - 0.356
      - -0.28
   - - 0.166
      - -0.3
   - - -0.159
      - -0.3
   - - -0.166
      - 0.3
   - - 0.166
      - 0.3
   - - 0.356
      - 0.28
   - - 0.681
     - 0.28

I then tried writing to footprint parameter using cpp code like this:

    std::string footprint_str;
    footprint_str="[[0.681,-0.28],[0.356,-0.28],[0.166,-0.3],[-0.159,-0.3],[-0.166,0.3],[0.166,0.3],[0.356,0.28],[0.681,0.28]]";
    ros::param::set("/move_base/local_costmap/footprint", footprint_str);
    ROS_INFO("Footprint: %s",footprint_str.c_str());

This doesn't work either. Although if you read/get param footprint you can see cpp code has actually changed the param but rviz doesn't reflect it. After adjusting footprint via param when you echo /move_base_local/footprint is doesn't show the change, but if your running rtq_reconfigure it does show a change but not in rviz. But if you close and open rqt_reconfigure the footprint is unchanged.

Also lots of people are saying your not suppose to use param's for constantly changing variables, that's what topics are for. I've looked at all theses links:

https ... (more)

edit retag flag offensive close merge delete

Comments

Hi there, I'm still searching for the correct way to do this, cant find the answer. i found this link https://answers.ros.org/question/237365/dynamic-footprint-in-navigation/, the problem with publishing to the footprint topic is its frame ID is odom so the polygon coordinates are changing depending on robots position. And here how-can-i-change-costmap-footprint-dynamically Surely someone has required a dynamic footprint? Thanks again.

Zonared ( 2023-05-29 07:49:12 -0500 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2023-07-27 12:59:03 -0500

burakaksoy
11 ●1 ●3 ●4

Inspired from Zonared's answer this is what I use:

import rospy
from dynamic_reconfigure.client import Client
from rospy.exceptions import ROSException
import time

def set_footprint(footprint):
    rospy.init_node("footprint_updater")

    new_config = {"footprint": footprint}

    service_wait_timeout = 2 # seconds
    num_trials = 5
    trial_intervals = 0.5 # seconds

    # Create a client for the global costmap
    try:
        rospy.wait_for_service("/move_base/global_costmap/set_parameters", timeout=service_wait_timeout)
        global_client = Client("/move_base/global_costmap")
        global_client.update_configuration(new_config)

        updated = False
        for _ in range(num_trials):
            time.sleep(trial_intervals)  # wait for the parameter to be updated
            updated_footprint = rospy.get_param("/move_base/global_costmap/footprint")
            if updated_footprint == str(footprint):
                updated = True
                break
        if not updated:
            rospy.logerr("Failed to update global costmap footprint.")
    except (ROSException, Exception) as e:
        rospy.logerr("Failed to create a client for global costmap or update its footprint: {}".format(e))

    # Create a client for the local costmap
    try:
        rospy.wait_for_service("/move_base/local_costmap/set_parameters", timeout=service_wait_timeout)
        local_client = Client("/move_base/local_costmap")
        local_client.update_configuration(new_config)

        updated = False
        for _ in range(num_trials):
            time.sleep(trial_intervals)  # wait for the parameter to be updated
            updated_footprint = rospy.get_param("/move_base/local_costmap/footprint")
            if updated_footprint == str(footprint):
                updated = True
                break
        if not updated:
            rospy.logerr("Failed to update local costmap footprint.")
    except (ROSException, Exception) as e:
        rospy.logerr("Failed to create a client for local costmap or update its footprint: {}".format(e))

def main():
    # Example polygon
    # footprint = [[1.0,-1.0], [1.0,1.0], [-1.0,1.0], [-1.0,-1.0]]
    footprint = [[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]
    set_footprint(footprint)

if __name__ == "__main__":
    main()

edit flag offensive delete link

add a comment

answered 2023-06-01 06:12:57 -0500

Zonared
48 ●5 ●6 ●12

I'm not sure if this is the correct way to solve it, or this should be the 'answer' but I managed to get my robot footprint to change dynamically via C++ by using dynamic_reconfigure dynparam, like this:

 std::string footprint_str = "[[0.83,-0.28],[0.356,-0.28],[0.166,-0.3],[-0.03,-0.3],[-0.03,0.3],[0.166,0.3],[0.356,0.28],[0.83,0.28]]";
 std::string footprint_node = "rosrun dynamic_reconfigure dynparam set /move_base/local_costmap footprint ";
 system((footprint_node + footprint_str).c_str());

This came from here UsingDynparamToChangeHokuyoLaserParameters (at the bottom of the page)

So when you change footprint_str (which I'm doing manually at the moment via a lookup table of sorts) the actual robot footprint changes to match it straightaway.

I would like to use something like this example but this code doesn't look complete and I don't know enough about point32 arrays, tf2::doTransform and then how to get this back to a string.

Long story short, it works good enough for me. Although now when I kill the node it says escalating to SIGTERM, probably due to the 'system' call, it'll work until a proper solution comes around. anyone??? ;)

edit flag offensive delete link

Comments

I believe I may have spoke too soon. Changing the footprint dynamically causes jumps in the robots position. It's like the map to odom TF changes each time the footprint is changed. :(

Zonared ( 2023-06-02 03:56:39 -0500 )edit

Me again, after more research and experimenting I've discovered the jumping around is related to the fact that the 'system' command holds up the whole process too much, like a blocking function. So I farmed it out to a python node using Python dynamic reconfigure

Like this:

#!/usr/bin/python3
import rospy
from std_msgs.msg import Float32
import dynamic_reconfigure.client

rospy.wait_for_service('/move_base/local_costmap/set_parameters')
client = dynamic_reconfigure.client.Client("/move_base/local_costmap", timeout=2, config_callback=None)

def callback(data):
    footprint_str="points in here"
    client.update_configuration({"footprint":footprint_str})

def listener():
    rospy.init_node('dyn_fp')#, anonymous=True)
    rospy.spin()

if __name__ == '__main_
    listener()

Works great. Now onto the next tricky thing, navigation.

Zonared ( 2023-06-12 05:57:00 -0500 )edit

add a comment

what is the correct way to change dynamic footprint?

Comments

2 Answers

Comments

Question Tools

Stats

Related questions

what is the correct way to change dynamic footprint? edit

Comments

2 Answers

Comments

Question Tools

Stats

Related questions

what is the correct way to change dynamic footprint?