Skip to content

Commit

Permalink
PSM: simplify state_update_pending_ (moveit#3682)
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>
  • Loading branch information
@rhaschke @v4hn
rhaschke and v4hn authored Jan 10, 2025
1 parent 4f36135 commit b71e5f5
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 77 deletions.
22 changes: 11 additions & 11 deletions ...ing/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -554,15 +554,19 @@ class PlanningSceneMonitor : private boost::noncopyable
bool getPlanningSceneServiceCallback(moveit_msgs::GetPlanningScene::Request& req,
moveit_msgs::GetPlanningScene::Response& res);

// Lock for state_update_pending_ and dt_state_update_
boost::mutex state_pending_mutex_;
/// True if current_state_monitor_ has a newer RobotState than scene_
std::atomic<bool> state_update_pending_;

/// True when we need to update the RobotState from current_state_monitor_
// This field is protected by state_pending_mutex_
volatile bool state_update_pending_;
// Lock for writing last_robot_state_update_wall_time_ and dt_state_update_
boost::mutex state_update_mutex_;

/// Last time the state was updated from current_state_monitor_
// Only access this from callback functions (and constructor)
// This field is protected by state_update_mutex_
ros::WallTime last_robot_state_update_wall_time_;

/// the amount of time to wait in between updates to the robot state
// This field is protected by state_pending_mutex_
// This field is protected by state_update_mutex_
ros::WallDuration dt_state_update_;

/// the amount of time to wait when looking up transforms
Expand All @@ -571,14 +575,10 @@ class PlanningSceneMonitor : private boost::noncopyable
ros::Duration shape_transform_cache_lookup_wait_time_;

/// timer for state updates.
// Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
// If state_update_pending_ is true, call updateSceneWithCurrentState()
// Not safe to access from callback functions.
ros::WallTimer state_update_timer_;

/// Last time the state was updated from current_state_monitor_
// Only access this from callback functions (and constructor)
ros::WallTime last_robot_state_update_wall_time_;

robot_model_loader::RobotModelLoaderPtr rm_loader_;
moveit::core::RobotModelConstPtr robot_model_;

Expand Down
93 changes: 27 additions & 66 deletions moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -253,7 +253,7 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc

shape_transform_cache_lookup_wait_time_ = ros::Duration(temp_wait_time);

state_update_pending_ = false;
state_update_pending_.store(false);
state_update_timer_ = nh_.createWallTimer(dt_state_update_, &PlanningSceneMonitor::stateUpdateTimerCallback, this,
false, // not a oneshot timer
false); // do not start the timer yet
Expand Down Expand Up @@ -932,14 +932,8 @@ bool PlanningSceneMonitor::waitForCurrentRobotState(const ros::Time& t, double w
If waitForCurrentState failed, we didn't get any new state updates within wait_time. */
if (success)
{
boost::mutex::scoped_lock lock(state_pending_mutex_);
if (state_update_pending_) // enforce state update
{
state_update_pending_ = false;
last_robot_state_update_wall_time_ = ros::WallTime::now();
lock.unlock();
if (state_update_pending_.load()) // perform state update
updateSceneWithCurrentState();
}
return true;
}

Expand Down Expand Up @@ -1150,7 +1144,7 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top
current_state_monitor_->startStateMonitor(joint_states_topic);

{
boost::mutex::scoped_lock lock(state_pending_mutex_);
boost::mutex::scoped_lock lock(state_update_mutex_);
if (!dt_state_update_.isZero())
state_update_timer_.start();
}
Expand All @@ -1175,68 +1169,27 @@ void PlanningSceneMonitor::stopStateMonitor()
if (attached_collision_object_subscriber_)
attached_collision_object_subscriber_.shutdown();

// stop must be called with state_pending_mutex_ unlocked to avoid deadlock
state_update_timer_.stop();
{
boost::mutex::scoped_lock lock(state_pending_mutex_);
state_update_pending_ = false;
}
state_update_pending_.store(false);
}

void PlanningSceneMonitor::onStateUpdate(const sensor_msgs::JointStateConstPtr& /* joint_state */)
{
const ros::WallTime& n = ros::WallTime::now();
ros::WallDuration dt = n - last_robot_state_update_wall_time_;
state_update_pending_.store(true);

bool update = false;
{
boost::mutex::scoped_lock lock(state_pending_mutex_);

if (dt < dt_state_update_)
{
state_update_pending_ = true;
}
else
{
state_update_pending_ = false;
last_robot_state_update_wall_time_ = n;
update = true;
}
}
// run the state update with state_pending_mutex_ unlocked
if (update)
// Read access to last_robot_state_update_wall_time_ and dt_state_update_ is unprotected here
// as reading invalid values is not critical (just postpones the next state update)
// only update every dt_state_update_ seconds
if (ros::WallTime::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
updateSceneWithCurrentState(true);
}

void PlanningSceneMonitor::stateUpdateTimerCallback(const ros::WallTimerEvent& /*unused*/)
{
if (state_update_pending_)
{
bool update = false;

const ros::WallTime& n = ros::WallTime::now();
ros::WallDuration dt = n - last_robot_state_update_wall_time_;

{
// lock for access to dt_state_update_ and state_update_pending_
boost::mutex::scoped_lock lock(state_pending_mutex_);
if (state_update_pending_ && dt >= dt_state_update_)
{
state_update_pending_ = false;
last_robot_state_update_wall_time_ = ros::WallTime::now();
update = true;
ROS_DEBUG_STREAM_NAMED(LOGNAME,
"performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
}
}

// run the state update with state_pending_mutex_ unlocked
if (update)
{
updateSceneWithCurrentState();
ROS_DEBUG_NAMED(LOGNAME, "performPendingStateUpdate done");
}
}
// Read access to last_robot_state_update_wall_time_ and dt_state_update_ is unprotected here
// as reading invalid values is not critical (just postpones the next state update)
if (state_update_pending_.load() && ros::WallTime::now() - last_robot_state_update_wall_time_ >= dt_state_update_)
updateSceneWithCurrentState(true);
}

void PlanningSceneMonitor::octomapUpdateCallback()
Expand Down Expand Up @@ -1268,18 +1221,18 @@ void PlanningSceneMonitor::setStateUpdateFrequency(double hz)
bool update = false;
if (hz > std::numeric_limits<double>::epsilon())
{
boost::mutex::scoped_lock lock(state_pending_mutex_);
boost::mutex::scoped_lock lock(state_update_mutex_);
dt_state_update_.fromSec(1.0 / hz);
state_update_timer_.setPeriod(dt_state_update_);
state_update_timer_.start();
}
else
{
// stop must be called with state_pending_mutex_ unlocked to avoid deadlock
// stop must be called with state_update_mutex_ unlocked to avoid deadlock
state_update_timer_.stop();
boost::mutex::scoped_lock lock(state_pending_mutex_);
boost::mutex::scoped_lock lock(state_update_mutex_);
dt_state_update_ = ros::WallDuration(0, 0);
if (state_update_pending_)
if (state_update_pending_.load())
update = true;
}
ROS_INFO_NAMED(LOGNAME, "Updating internal planning scene state at most every %lf seconds", dt_state_update_.toSec());
Expand All @@ -1305,15 +1258,23 @@ void PlanningSceneMonitor::updateSceneWithCurrentState(bool skip_update_if_locke
boost::unique_lock<boost::shared_mutex> ulock(scene_update_mutex_, boost::defer_lock);
if (!skip_update_if_locked)
ulock.lock();
else if (!ulock.try_lock_for(boost::chrono::duration<double>(std::min(0.1, 0.1 * dt_state_update_.toSec()))))
// Return if we can't lock scene_update_mutex, thus not blocking CurrentStateMonitor
else if (!ulock.try_lock())
// Return if we can't lock scene_update_mutex within 100ms, thus not blocking CurrentStateMonitor too long
return;

last_update_time_ = last_robot_motion_time_ = current_state_monitor_->getCurrentStateTime();
ROS_DEBUG_STREAM_NAMED(LOGNAME, "robot state update " << fmod(last_robot_motion_time_.toSec(), 10.));
current_state_monitor_->setToCurrentState(scene_->getCurrentStateNonConst());
scene_->getCurrentStateNonConst().update(); // compute all transforms
}

// Update state_update_mutex_ and last_robot_state_update_wall_time_
{
boost::mutex::scoped_lock lock(state_update_mutex_);
last_robot_state_update_wall_time_ = ros::WallTime::now();
state_update_pending_.store(false);
}

triggerSceneUpdateEvent(UPDATE_STATE);
}
else
Expand Down

0 comments on commit b71e5f5

Please sign in to comment.