Revision history [back]

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()