what is the correct way to change dynamic footprint?
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 /movebase/localcostmap/footprint because its frameid is odom, you also cant publish to movebase/globalcostmap/footprint because its frameid 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 /movebase/localcostmap/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 /movebaselocal/footprint is doesn't show the change, but if your running rtqreconfigure it does show a change but not in rviz. But if you close and open rqtreconfigure 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://answers.ros.org/question/227354/dynamic-parameters-update-from-the-code-cpp/
https://answers.ros.org/question/391200/unable-to-update-footprint-dynamically-from-c/
https://github.com/ros-planning/navigation2/issues/2626
How do you use dynamic footprint??? help please.
Asked by Zonared on 2023-05-06 01:32:09 UTC
Answers
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??? ;)
Asked by Zonared on 2023-06-01 06:12:57 UTC
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. :(
Asked by Zonared on 2023-06-02 03:56:39 UTC
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.
Asked by Zonared on 2023-06-12 05:57:00 UTC
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()
Asked by burakaksoy on 2023-07-27 12:59:03 UTC
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.
Asked by Zonared on 2023-05-29 07:49:12 UTC