Skip to content
This repository was archived by the owner on Nov 13, 2017. It is now read-only.

Commit 9479afc

Browse files
alainsanguinettiv4hn
alainsanguinetti
authored and
v4hn
committed
Added support for nodes handles with specific callbacks queues (#701)
1 parent 5fffdf3 commit 9479afc

File tree

6 files changed

+74
-22
lines changed

6 files changed

+74
-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
@@ -103,15 +103,21 @@ robot_model::RobotModelConstPtr getSharedRobotModel(const std::string &robot_des
103103
}
104104
}
105105

106-
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf)
106+
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf )
107+
{
108+
return getSharedStateMonitor( kmodel, tf, ros::NodeHandle() );
109+
}
110+
111+
planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr &kmodel, const boost::shared_ptr<tf::Transformer> &tf,
112+
ros::NodeHandle nh )
107113
{
108114
SharedStorage &s = getSharedStorage();
109115
boost::mutex::scoped_lock slock(s.lock_);
110116
if (s.state_monitors_.find(kmodel->getName()) != s.state_monitors_.end())
111117
return s.state_monitors_[kmodel->getName()];
112118
else
113119
{
114-
planning_scene_monitor::CurrentStateMonitorPtr monitor(new planning_scene_monitor::CurrentStateMonitor(kmodel, tf));
120+
planning_scene_monitor::CurrentStateMonitorPtr monitor(new planning_scene_monitor::CurrentStateMonitor(kmodel, tf, nh ));
115121
s.state_monitors_[kmodel->getName()] = monitor;
116122
return monitor;
117123
}

‎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: 10 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,29 @@ 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+
// explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
166+
( ( ros::CallbackQueue * ) node_handle_.getCallbackQueue())->callAvailable();
166167
}
167168

168169
// wait for the server (and spin as needed)
169170
if (wait_for_server == ros::Duration(0, 0))
170171
{
171172
while (node_handle_.ok() && !action->isServerConnected())
172173
{
173-
ros::WallDuration(0.02).sleep();
174-
ros::spinOnce();
174+
ros::WallDuration(0.001).sleep();
175+
// explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
176+
( ( ros::CallbackQueue * ) node_handle_.getCallbackQueue())->callAvailable();
175177
}
176178
}
177179
else
178180
{
179181
ros::Time final_time = ros::Time::now() + wait_for_server;
180182
while (node_handle_.ok() && !action->isServerConnected() && final_time > ros::Time::now())
181183
{
182-
ros::WallDuration(0.02).sleep();
183-
ros::spinOnce();
184+
ros::WallDuration(0.001).sleep();
185+
// explicit ros::spinOnce on the callback queue used by NodeHandle that manages the action client
186+
( ( ros::CallbackQueue * ) node_handle_.getCallbackQueue())->callAvailable();
184187
}
185188
}
186189

0 commit comments

Comments
 (0)