Skip to content

Commit ba35aaa

Browse files
riv-mjohnsonrhaschkev4hnsea-basssjahr
authored
* Use separate callback queue + spinner for ExecuteTrajectoryAction (moveit#3676) This allows parallel execution + planning. Also required modifying updateSceneWithCurrentState() to allow skipping a scene update with a new robot state (from CurrentStateMonitor), if the planning scene is currently locked (due to planning). Otherwise, the CurrentStateMonitor would block too. Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de> * PSM: simplify state_update_pending_ (moveit#3682) * Move update of state_update_pending_ to updateSceneWithCurrentState() * Revert to try_lock While there are a few other locks except explicit user locks (getPlanningSceneServiceCallback(), collisionObjectCallback(), attachObjectCallback(), newPlanningSceneCallback(), and scenePublishingThread()), these occur rather seldom (scenePublishingThread() publishes at 2Hz). Hence, we might indeed balance a non-blocking CSM vs. missed PS updates in favour of CSM. * Don't block for scene update from stateUpdateTimerCallback too The timer callback and CSM's state update callbacks are served from the same callback queue, which would block CSM again. * further locking adaptations reading dt_state_update_ and last_robot_state_update_wall_time_ does not lead to logic errors, but at most to a skipped or redundant update on corrupted data. Alternatively we could be on the safe side and turn both variables into std::atomic, but that would effectively mean locks on every read. Instead, only set state_update_pending_ as an atomic, which is lockfree in this case. Co-authored-by: Michael Görner <me@v4hn.de> * Ports changes to ROS2. --------- Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de> Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com> Co-authored-by: Michael Görner <me@v4hn.de> Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>
1 parent fbdd8c5 commit ba35aaa

4 files changed

Lines changed: 72 additions & 80 deletions

File tree

‎moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp‎

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,9 +57,21 @@ MoveGroupExecuteTrajectoryAction::MoveGroupExecuteTrajectoryAction() : MoveGroup
5757
{
5858
}
5959

60+
MoveGroupExecuteTrajectoryAction::~MoveGroupExecuteTrajectoryAction()
61+
{
62+
callback_executor_.cancel();
63+
64+
if (callback_thread_.joinable())
65+
callback_thread_.join();
66+
}
67+
6068
void MoveGroupExecuteTrajectoryAction::initialize()
6169
{
6270
auto node = context_->moveit_cpp_->getNode();
71+
callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive,
72+
false /* don't spin with node executor */);
73+
callback_executor_.add_callback_group(callback_group_, node->get_node_base_interface());
74+
callback_thread_ = std::thread([this]() { callback_executor_.spin(); });
6375
// start the move action server
6476
execute_action_server_ = rclcpp_action::create_server<ExecTrajectory>(
6577
node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(),
@@ -68,7 +80,8 @@ void MoveGroupExecuteTrajectoryAction::initialize()
6880
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
6981
},
7082
[](const std::shared_ptr<ExecTrajectoryGoal>& /* unused */) { return rclcpp_action::CancelResponse::ACCEPT; },
71-
[this](const auto& goal) { executePathCallback(goal); });
83+
[this](const auto& goal) { executePathCallback(goal); }, rcl_action_server_get_default_options(),
84+
callback_group_);
7285
}
7386

7487
void MoveGroupExecuteTrajectoryAction::executePathCallback(const std::shared_ptr<ExecTrajectoryGoal>& goal)

‎moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@ class MoveGroupExecuteTrajectoryAction : public MoveGroupCapability
5555
{
5656
public:
5757
MoveGroupExecuteTrajectoryAction();
58+
~MoveGroupExecuteTrajectoryAction() override;
5859

5960
void initialize() override;
6061

@@ -65,6 +66,11 @@ class MoveGroupExecuteTrajectoryAction : public MoveGroupCapability
6566
void preemptExecuteTrajectoryCallback();
6667
void setExecuteTrajectoryState(MoveGroupState state, const std::shared_ptr<ExecTrajectoryGoal>& goal);
6768

69+
// Use our own callback group and executor to allow execution parallel to other capabilities
70+
rclcpp::CallbackGroup::SharedPtr callback_group_;
71+
rclcpp::executors::SingleThreadedExecutor callback_executor_;
72+
std::thread callback_thread_;
73+
6874
std::shared_ptr<rclcpp_action::Server<ExecTrajectory>> execute_action_server_;
6975
};
7076

‎moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -296,8 +296,9 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
296296

297297
/** @brief Update the scene using the monitored state. This function is automatically called when an update to the
298298
current state is received (if startStateMonitor() has been called).
299-
The updates are throttled to a maximum update frequency however, which is set by setStateUpdateFrequency(). */
300-
void updateSceneWithCurrentState();
299+
The updates are throttled to a maximum update frequency however, which is set by setStateUpdateFrequency().
300+
@param skip_update_if_locked causes the update to be skipped if the planning scene is locked. */
301+
void updateSceneWithCurrentState(bool skip_update_if_locked = false);
301302

302303
/** @brief Update the scene using the monitored state at a specified frequency, in Hz. This function has an effect
303304
only when updates from the CurrentStateMonitor are received at a higher frequency.
@@ -557,15 +558,19 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
557558
void updatePublishSettings(bool publish_geom_updates, bool publish_state_updates, bool publish_transform_updates,
558559
bool publish_planning_scene, double publish_planning_scene_hz);
559560

560-
// Lock for state_update_pending_ and dt_state_update_
561-
std::mutex state_pending_mutex_;
561+
/// True if current_state_monitor_ has a newer RobotState than scene_
562+
std::atomic<bool> state_update_pending_;
562563

563-
/// True when we need to update the RobotState from current_state_monitor_
564-
// This field is protected by state_pending_mutex_
565-
volatile bool state_update_pending_;
564+
// Lock for writing last_robot_state_update_wall_time_ and dt_state_update_
565+
std::mutex state_update_mutex_;
566+
567+
/// Last time the state was updated from current_state_monitor_
568+
// Only access this from callback functions (and constructor)
569+
// This field is protected by state_update_mutex_
570+
std::chrono::system_clock::time_point last_robot_state_update_wall_time_;
566571

567572
/// the amount of time to wait in between updates to the robot state
568-
// This field is protected by state_pending_mutex_
573+
// This field is protected by state_update_mutex_
569574
std::chrono::duration<double> dt_state_update_; // 1hz
570575

571576
/// the amount of time to wait when looking up transforms
@@ -574,15 +579,10 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor
574579
rclcpp::Duration shape_transform_cache_lookup_wait_time_;
575580

576581
/// timer for state updates.
577-
// Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
582+
// If state_update_pending_ is true, call updateSceneWithCurrentState()
578583
// Not safe to access from callback functions.
579-
580584
rclcpp::TimerBase::SharedPtr state_update_timer_;
581585

582-
/// Last time the state was updated from current_state_monitor_
583-
// Only access this from callback functions (and constructor)
584-
std::chrono::system_clock::time_point last_robot_state_update_wall_time_;
585-
586586
robot_model_loader::RobotModelLoaderPtr rm_loader_;
587587
moveit::core::RobotModelConstPtr robot_model_;
588588

‎moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp‎

Lines changed: 38 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -223,7 +223,7 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
223223

224224
shape_transform_cache_lookup_wait_time_ = rclcpp::Duration::from_seconds(temp_wait_time);
225225

226-
state_update_pending_ = false;
226+
state_update_pending_.store(false);
227227
// Period for 0.1 sec
228228
using std::chrono::nanoseconds;
229229
state_update_timer_ = pnode_->create_wall_timer(dt_state_update_, [this]() { return stateUpdateTimerCallback(); });
@@ -1110,12 +1110,8 @@ bool PlanningSceneMonitor::waitForCurrentRobotState(const rclcpp::Time& t, doubl
11101110
If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
11111111
if (success)
11121112
{
1113-
std::unique_lock<std::mutex> lock(state_pending_mutex_);
1114-
if (state_update_pending_) // enforce state update
1113+
if (state_update_pending_.load()) // perform state update
11151114
{
1116-
state_update_pending_ = false;
1117-
last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
1118-
lock.unlock();
11191115
updateSceneWithCurrentState();
11201116
}
11211117
return true;
@@ -1354,7 +1350,7 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top
13541350
current_state_monitor_->startStateMonitor(joint_states_topic);
13551351

13561352
{
1357-
std::unique_lock<std::mutex> lock(state_pending_mutex_);
1353+
std::unique_lock<std::mutex> lock(state_update_mutex_);
13581354
if (dt_state_update_.count() > 0)
13591355
{
13601356
// ROS original: state_update_timer_.start();
@@ -1387,69 +1383,29 @@ void PlanningSceneMonitor::stopStateMonitor()
13871383
if (attached_collision_object_subscriber_)
13881384
attached_collision_object_subscriber_.reset();
13891385

1390-
// stop must be called with state_pending_mutex_ unlocked to avoid deadlock
13911386
if (state_update_timer_)
13921387
state_update_timer_->cancel();
1393-
{
1394-
std::unique_lock<std::mutex> lock(state_pending_mutex_);
1395-
state_update_pending_ = false;
1396-
}
1388+
state_update_pending_.store(false);
13971389
}
13981390

13991391
void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::msg::JointState::ConstSharedPtr& /*joint_state */)
14001392
{
1401-
const std::chrono::system_clock::time_point& n = std::chrono::system_clock::now();
1402-
std::chrono::duration<double> dt = n - last_robot_state_update_wall_time_;
1403-
1404-
bool update = false;
1405-
{
1406-
std::unique_lock<std::mutex> lock(state_pending_mutex_);
1393+
state_update_pending_.store(true);
14071394

1408-
if (dt.count() < dt_state_update_.count())
1409-
{
1410-
state_update_pending_ = true;
1411-
}
1412-
else
1413-
{
1414-
state_update_pending_ = false;
1415-
last_robot_state_update_wall_time_ = n;
1416-
update = true;
1417-
}
1418-
}
1419-
// run the state update with state_pending_mutex_ unlocked
1420-
if (update)
1421-
updateSceneWithCurrentState();
1395+
// Read access to last_robot_state_update_wall_time_ and dt_state_update_ is unprotected here
1396+
// as reading invalid values is not critical (just postpones the next state update)
1397+
// only update every dt_state_update_ seconds
1398+
if (std::chrono::system_clock::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
1399+
updateSceneWithCurrentState(true);
14221400
}
14231401

14241402
void PlanningSceneMonitor::stateUpdateTimerCallback()
14251403
{
1426-
if (state_update_pending_)
1427-
{
1428-
bool update = false;
1429-
1430-
std::chrono::system_clock::time_point n = std::chrono::system_clock::now();
1431-
std::chrono::duration<double> dt = n - last_robot_state_update_wall_time_;
1432-
1433-
{
1434-
// lock for access to dt_state_update_ and state_update_pending_
1435-
std::unique_lock<std::mutex> lock(state_pending_mutex_);
1436-
if (state_update_pending_ && dt.count() >= dt_state_update_.count())
1437-
{
1438-
state_update_pending_ = false;
1439-
last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
1440-
auto sec = std::chrono::duration<double>(last_robot_state_update_wall_time_.time_since_epoch()).count();
1441-
update = true;
1442-
RCLCPP_DEBUG(logger_, "performPendingStateUpdate: %f", fmod(sec, 10));
1443-
}
1444-
}
1445-
1446-
// run the state update with state_pending_mutex_ unlocked
1447-
if (update)
1448-
{
1449-
updateSceneWithCurrentState();
1450-
RCLCPP_DEBUG(logger_, "performPendingStateUpdate done");
1451-
}
1452-
}
1404+
// Read access to last_robot_state_update_wall_time_ and dt_state_update_ is unprotected here
1405+
// as reading invalid values is not critical (just postpones the next state update)
1406+
if (state_update_pending_.load() &&
1407+
std::chrono::system_clock::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
1408+
updateSceneWithCurrentState(true);
14531409
}
14541410

14551411
void PlanningSceneMonitor::octomapUpdateCallback()
@@ -1481,22 +1437,22 @@ void PlanningSceneMonitor::setStateUpdateFrequency(double hz)
14811437
bool update = false;
14821438
if (hz > std::numeric_limits<double>::epsilon())
14831439
{
1484-
std::unique_lock<std::mutex> lock(state_pending_mutex_);
1440+
std::unique_lock<std::mutex> lock(state_update_mutex_);
14851441
dt_state_update_ = std::chrono::duration<double>(1.0 / hz);
14861442
// ROS original: state_update_timer_.start();
14871443
// TODO: re-enable WallTimer start()
14881444
state_update_timer_ = pnode_->create_wall_timer(dt_state_update_, [this]() { return stateUpdateTimerCallback(); });
14891445
}
14901446
else
14911447
{
1492-
// stop must be called with state_pending_mutex_ unlocked to avoid deadlock
1448+
// stop must be called with state_update_mutex_ unlocked to avoid deadlock
14931449
// ROS original: state_update_timer_.stop();
14941450
// TODO: re-enable WallTimer stop()
14951451
if (state_update_timer_)
14961452
state_update_timer_->cancel();
1497-
std::unique_lock<std::mutex> lock(state_pending_mutex_);
1453+
std::unique_lock<std::mutex> lock(state_update_mutex_);
14981454
dt_state_update_ = std::chrono::duration<double>(0.0);
1499-
if (state_update_pending_)
1455+
if (state_update_pending_.load())
15001456
update = true;
15011457
}
15021458
RCLCPP_INFO(logger_, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.count());
@@ -1505,7 +1461,7 @@ void PlanningSceneMonitor::setStateUpdateFrequency(double hz)
15051461
updateSceneWithCurrentState();
15061462
}
15071463

1508-
void PlanningSceneMonitor::updateSceneWithCurrentState()
1464+
void PlanningSceneMonitor::updateSceneWithCurrentState(bool skip_update_if_locked)
15091465
{
15101466
rclcpp::Time time = node_->now();
15111467
rclcpp::Clock steady_clock = rclcpp::Clock(RCL_STEADY_TIME);
@@ -1524,12 +1480,29 @@ void PlanningSceneMonitor::updateSceneWithCurrentState()
15241480
}
15251481

15261482
{
1527-
std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_);
1483+
std::unique_lock<std::shared_mutex> ulock(scene_update_mutex_, std::defer_lock);
1484+
if (!skip_update_if_locked)
1485+
{
1486+
ulock.lock();
1487+
}
1488+
else if (!ulock.try_lock())
1489+
{
1490+
// Return if we can't lock scene_update_mutex, thus not blocking CurrentStateMonitor
1491+
return;
1492+
}
15281493
last_update_time_ = last_robot_motion_time_ = current_state_monitor_->getCurrentStateTime();
15291494
RCLCPP_DEBUG(logger_, "robot state update %f", fmod(last_robot_motion_time_.seconds(), 10.));
15301495
current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
15311496
scene_->getCurrentStateNonConst().update(); // compute all transforms
15321497
}
1498+
1499+
// Update state_update_mutex_ and last_robot_state_update_wall_time_
1500+
{
1501+
std::unique_lock<std::mutex> lock(state_update_mutex_);
1502+
last_robot_state_update_wall_time_ = std::chrono::system_clock::now();
1503+
state_update_pending_.store(false);
1504+
}
1505+
15331506
triggerSceneUpdateEvent(UPDATE_STATE);
15341507
}
15351508
else

0 commit comments

Comments
 (0)