Skip to content

Commit

Permalink
renamed CurrentStateMonitor::waitForCurrentState() to waitForComplete…
Browse files Browse the repository at this point in the history
…State() (moveit#298)

.. to better reflect the actual semantics of the method
to maintain API compatibility, the old function still exists, but is deprecated
Conflicts:
	moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h
	moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
	moveit_ros/planning_interface/move_group_interface/src/move_group.cpp
  • Loading branch information
rhaschke authored and k-okada committed Dec 12, 2017
1 parent 8abb0b6 commit eead1c3
Show file tree
Hide file tree
Showing 5 changed files with 23 additions and 9 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <moveit/macros/deprecation.h>

namespace planning_scene_monitor
{
Expand Down Expand Up @@ -136,11 +137,15 @@ class CurrentStateMonitor

/** @brief Wait for at most \e wait_time seconds until the complete current state is known. Return true if the full
* state is known */
bool waitForCurrentState(double wait_time) const;
bool waitForCompleteState(double wait_time) const;
/** replaced by waitForCompleteState, will be removed in L-turtle: function waits for complete robot state */
MOVEIT_DEPRECATED bool waitForCurrentState(double wait_time) const;

/** @brief Wait for at most \e wait_time seconds until the joint values from the group \e group are known. Return true
* if values for all joints in \e group are known */
bool waitForCurrentState(const std::string& group, double wait_time) const;
bool waitForCompleteState(const std::string &group, double wait_time) const;
/** replaced by waitForCompleteState, will be removed in L-turtle: function waits for complete robot state */
MOVEIT_DEPRECATED bool waitForCurrentState(const std::string &group, double wait_time) const;

/** @brief Get the time point when the monitor was started */
const ros::Time& getMonitorStartTime() const
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -261,7 +261,7 @@ bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(const ros::D
return result;
}

bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(double wait_time) const
bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(double wait_time) const
{
double slept_time = 0.0;
double sleep_step_s = std::min(0.05, wait_time / 10.0);
Expand All @@ -273,10 +273,14 @@ bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(double wai
}
return haveCompleteState();
}
bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(double wait_time) const
{
waitForCompleteState(wait_time);
}

bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const std::string& group, double wait_time) const
bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState(const std::string &group, double wait_time) const
{
if (waitForCurrentState(wait_time))
if (waitForCompleteState(wait_time))
return true;
bool ok = true;

Expand All @@ -301,6 +305,11 @@ bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const std:
return ok;
}

bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const std::string &group, double wait_time) const
{
waitForCompleteState(group, wait_time);
}

void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
{
if (joint_state->name.size() != joint_state->position.size())
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -932,7 +932,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont
ROS_DEBUG_NAMED("traj_execution", "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);

robot_state::RobotStatePtr current_state;
if (!csm_->waitForCurrentState(1.0) || !(current_state = csm_->getCurrentState()))
if (!csm_->waitForCompleteState(1.0) || !(current_state = csm_->getCurrentState()))
{
ROS_WARN_NAMED("traj_execution", "Failed to validate trajectory: couldn't receive full current joint state within "
"1s");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -572,7 +572,7 @@ class MoveGroup::MoveGroupImpl
if (!current_state_monitor_->isActive())
current_state_monitor_->startStateMonitor();

current_state_monitor_->waitForCurrentState(opt_.group_name_, wait);
current_state_monitor_->waitForCompleteState(opt_.group_name_, wait);
return true;
}

Expand All @@ -588,7 +588,7 @@ class MoveGroup::MoveGroupImpl
if (!current_state_monitor_->isActive())
current_state_monitor_->startStateMonitor();

if (!current_state_monitor_->waitForCurrentState(opt_.group_name_, wait_seconds))
if (!current_state_monitor_->waitForCompleteState(opt_.group_name_, wait_seconds))
ROS_WARN_NAMED("move_group_interface", "Joint values for monitored state are requested but the full state is not "
"known");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -190,7 +190,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer
if (!current_state_monitor_->isActive())
{
current_state_monitor_->startStateMonitor();
if (!current_state_monitor_->waitForCurrentState(wait))
if (!current_state_monitor_->waitForCompleteState(wait))
ROS_WARN("Joint values for monitored state are requested but the full state is not known");
}
return true;
Expand Down

0 comments on commit eead1c3

Please sign in to comment.