search

rayvburn / humap_local_planner

Human-aware robot trajectory planner using a hybrid trajectory candidates generation and spatiotemporal cost functions
BSD 3-Clause "New" or "Revised" License
0 stars 1 forks source link

consider copying objects of the sparse environment model to avoid using mutexes for the whole planning procedure #118

Closed rayvburn closed 1 year ago

rayvburn commented 1 year ago

Currently, when it takes for the planner to compute any longer than usual, this kind of warning appears and the next control loop is omitted - zero velocity is directly applied:

observation buffer has not been updated for X seconds, and it should be updated every Y seconds.
rayvburn commented 1 year ago

It seems that the move_base's callbacks cannot be processed while the planner calculates new velocity command. This seems to be related to the mutex in the peopleCallback in the ROS interface of the planner.

rayvburn commented 1 year ago 
[ INFO] [1695504967.634739396, 22.151000000]: Full control cycle time: real {1.843438527}, sim {0.901000000}
[ WARN] [1695504967.634773651, 22.151000000]: Control loop missed its desired rate of 2.2500Hz... the loop actually took 0.9011 seconds
[ WARN] [1695504967.634809720, 22.151000000]: The scan_narrow observation buffer has not been updated for 0.97 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1695504967.634851551, 22.152000000]: [/move_base]:Sensor data is out of date, we're not going to allow commanding of the base for safety
[ INFO] [1695504967.634915089, 22.152000000]: Full control cycle time: real {0.000103491}, sim {0.001000000}
[ WARN] [1695504967.637096423, 22.152000000]: The rgbd_scan observation buffer has not been updated for 0.93 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1695504967.666579783, 22.159000000]: Map update loop missed its desired rate of 2.2500Hz... the loop actually took 0.8891 seconds
rayvburn commented 1 year ago

Data received from callbacks are saved. Then, in the main "thread" (where computeVelocityCommands are called), everything is updated according to the data received asynchronously. No excessive mutexes are present - no callback blocking. Data updated asynchronously are processed on demand only (in time, when needed).