Skip to content

Commit 02e7118

Browse files
author
Alain Sanguinetti
committed
Added support for nodes handles with specific callbacks queues
pr#701 updated code PR moveit#701 typo fixes and overload for getSharedStateMonitor
1 parent 73c9358 commit 02e7118

File tree

6 files changed

+71
-22
lines changed

6 files changed

+71
-22
lines changed

‎planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -56,11 +56,20 @@ class CurrentStateMonitor
5656
{
5757
public:
5858

59-
/** @brief Constructor
59+
/**
60+
* @brief Constructor.
61+
* @param robot_model The current kinematic model to build on
62+
* @param tf A pointer to the tf transformer to use
63+
*/
64+
CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf );
65+
66+
/** @brief Constructor.
6067
* @param robot_model The current kinematic model to build on
6168
* @param tf A pointer to the tf transformer to use
69+
* @param nh A ros::NodeHandle to pass node specific options
6270
*/
63-
CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf);
71+
CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf,
72+
ros::NodeHandle nh );
6473

6574
~CurrentStateMonitor();
6675

‎planning/planning_scene_monitor/src/current_state_monitor.cpp‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,8 +38,15 @@
3838
#include <tf_conversions/tf_eigen.h>
3939
#include <limits>
4040

41-
planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf)
42-
: tf_(tf)
41+
planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf )
42+
: CurrentStateMonitor( robot_model, tf, ros::NodeHandle() )
43+
{
44+
}
45+
46+
planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr<tf::Transformer> &tf,
47+
ros::NodeHandle nh )
48+
: nh_( nh )
49+
, tf_(tf)
4350
, robot_model_(robot_model)
4451
, robot_state_(robot_model)
4552
, state_monitor_started_(false)

‎planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h‎

Lines changed: 24 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -49,9 +49,29 @@ boost::shared_ptr<tf::Transformer> getSharedTF();
4949

5050
robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_description);
5151

52-
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf);
52+
/**
53+
@brief getSharedStateMonitor is a simpler version of getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf,
54+
ros::NodeHandle nh = ros::NodeHandle() ). It calls this function using the default constructed ros::NodeHandle
5355
54-
}
55-
}
56+
@param kmodel
57+
@param tf
58+
@return
59+
*/
60+
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf );
5661

57-
#endif
62+
/**
63+
@brief getSharedStateMonitor
64+
65+
@param kmodel
66+
@param tf
67+
@param nh A ros::NodeHandle to pass node specific configurations, such as callbacks queues.
68+
@return
69+
*/
70+
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf,
71+
ros::NodeHandle nh );
72+
73+
74+
} // namespace planning interface
75+
} // namespace moveit
76+
77+
#endif // end of MOVEIT_PLANNING_INTERFACE_COMMON_OBJECTS_

‎planning_interface/common_planning_interface_objects/src/common_objects.cpp‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -96,15 +96,21 @@ robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_des
9696
}
9797
}
9898

99-
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf)
99+
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf )
100+
{
101+
return getSharedStateMonitor( kmodel, tf, ros::NodeHandle() );
102+
}
103+
104+
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf,
105+
ros::NodeHandle nh )
100106
{
101107
SharedStorage &s = getSharedStorage();
102108
boost::mutex::scoped_lock slock(s.lock_);
103109
if (s.state_monitors_.find(kmodel->getName()) != s.state_monitors_.end())
104110
return s.state_monitors_[kmodel->getName()];
105111
else
106112
{
107-
planning_scene_monitor::CurrentStateMonitorPtr monitor(new planning_scene_monitor::CurrentStateMonitor(kmodel, tf));
113+
planning_scene_monitor::CurrentStateMonitorPtr monitor(new planning_scene_monitor::CurrentStateMonitor(kmodel, tf, nh ));
108114
s.state_monitors_[kmodel->getName()] = monitor;
109115
return monitor;
110116
}

‎planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h‎

Lines changed: 12 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -110,15 +110,22 @@ class MoveGroup
110110
double planning_time_;
111111
};
112112

113-
/** \brief Construct a client for the MoveGroup action using a specified set of options \e opt. Optionally, specify a TF instance to use.
114-
If not specified, one will be constructed internally. A timeout for connecting to the action server can also be specified. If it is not specified,
115-
the wait time is unlimited. */
113+
/**
114+
\brief Construct a client for the MoveGroup action using a specified set of options \e opt.
115+
116+
\param opt. A MoveGroup::Options structure, if you pass a ros::NodeHandle with a specific callback queue, it has to be of type ros::CallbackQueue
117+
(which is the default type of callback queues used in ROS)
118+
\param tf. Specify a TF instance to use. If not specified, one will be constructed internally.
119+
\param wait_for_server. Optional timeout for connecting to the action server. If it is not specified, the wait time is unlimited.
120+
*/
116121
MoveGroup(const Options &opt, const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
117122
const ros::Duration &wait_for_server = ros::Duration(0, 0));
118123

119-
/** \brief Construct a client for the MoveGroup action for a particular \e group. Optionally, specify a TF instance to use.
124+
/**
125+
\brief Construct a client for the MoveGroup action for a particular \e group. Optionally, specify a TF instance to use.
120126
If not specified, one will be constructed internally. A timeout for connecting to the action server can also be specified. If it is not specified,
121-
the wait time is unlimited. */
127+
the wait time is unlimited.
128+
*/
122129
MoveGroup(const std::string &group, const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
123130
const ros::Duration &wait_for_server = ros::Duration(0, 0));
124131

‎planning_interface/move_group_interface/src/move_group.cpp‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,7 @@ class MoveGroup::MoveGroupImpl
128128
trajectory_event_publisher_ = node_handle_.advertise<std_msgs::String>(trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1, false);
129129
attached_object_publisher_ = node_handle_.advertise<moveit_msgs::AttachedCollisionObject>(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1, false);
130130

131-
current_state_monitor_ = getSharedStateMonitor(robot_model_, tf_);
131+
current_state_monitor_ = getSharedStateMonitor( robot_model_, tf_, node_handle_ );
132132

133133
move_action_client_.reset(new actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>(node_handle_,
134134
move_group::MOVE_ACTION,
@@ -161,26 +161,26 @@ class MoveGroup::MoveGroupImpl
161161
ros::Time start_time = ros::Time::now();
162162
while (start_time == ros::Time::now())
163163
{
164-
ros::WallDuration(0.01).sleep();
165-
ros::spinOnce();
164+
ros::WallDuration(0.001).sleep();
165+
( ( ros::CallbackQueue * ) node_handle_.getCallbackQueue())->callAvailable();
166166
}
167167

168168
// wait for the server (and spin as needed)
169169
if (wait_for_server == ros::Duration(0, 0))
170170
{
171171
while (node_handle_.ok() && !action->isServerConnected())
172172
{
173-
ros::WallDuration(0.02).sleep();
174-
ros::spinOnce();
173+
ros::WallDuration(0.001).sleep();
174+
( ( ros::CallbackQueue * ) node_handle_.getCallbackQueue())->callAvailable();
175175
}
176176
}
177177
else
178178
{
179179
ros::Time final_time = ros::Time::now() + wait_for_server;
180180
while (node_handle_.ok() && !action->isServerConnected() && final_time > ros::Time::now())
181181
{
182-
ros::WallDuration(0.02).sleep();
183-
ros::spinOnce();
182+
ros::WallDuration(0.001).sleep();
183+
( ( ros::CallbackQueue * ) node_handle_.getCallbackQueue())->callAvailable();
184184
}
185185
}
186186

0 commit comments

Comments
 (0)