gazebo_ros_moveit_planning_scene.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 /*
18  * Desc: A plugin which publishes the gazebo world state as a MoveIt! planning scene
19  * Author: Jonathan Bohren
20  * Date: 15 May 2014
21  */
22 #ifndef GAZEBO_ROS_MOVEIT_PLANNING_SCENE_H
23 #define GAZEBO_ROS_MOVEIT_PLANNING_SCENE_H
24 
25 #include <string>
26 
27 // Custom Callback Queue
28 #include <ros/callback_queue.h>
29 #include <ros/subscribe_options.h>
30 #include <geometry_msgs/Pose.h>
31 
32 #include <ros/ros.h>
33 #include <boost/thread.hpp>
34 #include <boost/thread/mutex.hpp>
35 
36 #include <gazebo/physics/physics.hh>
37 #include <gazebo/transport/TransportTypes.hh>
38 #include <gazebo/common/Plugin.hh>
39 #include <gazebo/common/Events.hh>
40 
41 #include <std_srvs/Empty.h>
42 #include <moveit_msgs/PlanningScene.h>
43 
44 namespace gazebo
45 {
48 
91 class GazeboRosMoveItPlanningScene : public ModelPlugin
92 {
95 
97  public: virtual ~GazeboRosMoveItPlanningScene();
98 
99  // Documentation inherited
100  protected: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
101 
102  // Documentation inherited
103  protected: virtual void UpdateCB();
104 
105  protected: void subscriber_connected();
106 
108  private: bool PublishPlanningSceneCB(
109  std_srvs::Empty::Request& req,
110  std_srvs::Empty::Response& resp);
111 
113  private: void QueueThread();
114 
116  private: physics::WorldPtr world_;
117 
119  private: physics::ModelPtr model_;
120 
122  private: boost::scoped_ptr<ros::NodeHandle> rosnode_;
125 
127  private: boost::mutex mutex_;
128 
130  private: std::string topic_name_;
132  private: std::string scene_name_;
133  private: std::string robot_name_;
134  private: std::string model_name_;
135 
137  private: std::string robot_namespace_;
138 
139  // Custom Callback Queue
142  private: boost::thread callback_queue_thread_;
144  private: moveit_msgs::PlanningScene planning_scene_msg_;
145  std::map<std::string, moveit_msgs::CollisionObject> collision_object_map_;
146 
147  // Pointer to the update event connection
148  private: event::ConnectionPtr update_connection_;
149  private: event::ConnectionPtr add_connection_;
150  private: event::ConnectionPtr delete_connection_;
151 
152  private:
154 
157 };
159 }
161 #endif
bool PublishPlanningSceneCB(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
publish the complete planning scene
moveit_msgs::PlanningScene planning_scene_msg_
Container for the planning scene.
boost::mutex mutex_
A mutex to lock access to fields that are used in ROS message callbacks.
physics::WorldPtr world_
A pointer to the gazebo world.
std::string robot_namespace_
for setting ROS name space
std::string scene_name_
The MoveIt scene name.
std::string topic_name_
ROS topic name inputs.
physics::ModelPtr model_
A pointer to the Model of the robot doing the planning.
void QueueThread()
The custom callback queue thread function.
boost::scoped_ptr< ros::NodeHandle > rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
std::map< std::string, moveit_msgs::CollisionObject > collision_object_map_
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
boost::thread callback_queue_thread_
Thead object for the running callback Thread.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39