gazebo_ros_joint_trajectory.h
Go to the documentation of this file.
1 /*
2  * Copyright 2012 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 // *************************************************************
19 // DEPRECATED
20 // This class has been renamed to gazebo_ros_joint_pose_trajectory
21 // *************************************************************
22 
23 #ifndef GAZEBO_ROS_JOINT_TRAJECTORY_PLUGIN_HH
24 #define GAZEBO_ROS_JOINT_TRAJECTORY_PLUGIN_HH
25 
26 #include <string>
27 #include <vector>
28 
29 #include <boost/thread.hpp>
30 #include <boost/thread/mutex.hpp>
31 
32 #include <ros/ros.h>
33 #include <ros/callback_queue.h>
34 #include <ros/advertise_options.h>
35 #include <ros/subscribe_options.h>
36 
37 #include <trajectory_msgs/JointTrajectory.h>
38 #include <geometry_msgs/Pose.h>
39 
40 #undef ENABLE_SERVICE
41 #ifdef ENABLE_SERVICE
42 #include <gazebo_msgs/SetJointTrajectory.h>
43 #endif
44 
45 #include <gazebo/physics/physics.hh>
46 #include <gazebo/transport/TransportTypes.hh>
47 #include <gazebo/common/Time.hh>
48 #include <gazebo/common/Plugin.hh>
49 #include <gazebo/common/Events.hh>
50 
51 namespace gazebo
52 {
53  class GazeboRosJointTrajectory : public ModelPlugin // replaced with GazeboROSJointPoseTrajectory
54  {
56  public: GazeboRosJointTrajectory();
57 
59  public: virtual ~GazeboRosJointTrajectory();
60 
62  public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
63 
65  private: void SetTrajectory(
66  const trajectory_msgs::JointTrajectory::ConstPtr& trajectory);
67 #ifdef ENABLE_SERVICE
68  private: bool SetTrajectory(
69  const gazebo_msgs::SetJointTrajectory::Request& req,
70  const gazebo_msgs::SetJointTrajectory::Response& res);
71 #endif
72  private: void UpdateStates();
73 
74  private: physics::WorldPtr world_;
75  private: physics::ModelPtr model_;
76 
78  private: physics::LinkPtr reference_link_;
79  private: std::string reference_link_name_;
80 
85  private: bool has_trajectory_;
86 
88  private: trajectory_msgs::JointTrajectory trajectory_msg_;
89  private: bool set_model_pose_;
90  private: geometry_msgs::Pose model_pose_;
91 
93  private: std::string topic_name_;
94  private: std::string service_name_;
95 
98  private: boost::mutex update_mutex;
99 
101  private: common::Time last_time_;
102 
103  // trajectory time control
104  private: common::Time trajectory_start;
105  private: unsigned int trajectory_index;
106 
107  // rate control
108  private: double update_rate_;
110  private: bool physics_engine_enabled_;
111 
113  private: std::string robot_namespace_;
114 
116  private: void QueueThread();
117  private: boost::thread callback_queue_thread_;
118 
119  private: std::vector<gazebo::physics::JointPtr> joints_;
120  private: std::vector<trajectory_msgs::JointTrajectoryPoint> points_;
121 
122  // Pointer to the update event connection
123  private: event::ConnectionPtr update_connection_;
124 
125  private: trajectory_msgs::JointTrajectory joint_trajectory_;
126 
127  // deferred load in case ros is blocking
128  private: sdf::ElementPtr sdf;
129  private: void LoadThread();
130  private: boost::thread deferred_load_thread_;
131  };
132 }
133 #endif
boost::mutex update_mutex
A mutex to lock access to fields that are used in message callbacks.
trajectory_msgs::JointTrajectory joint_trajectory_
trajectory_msgs::JointTrajectory trajectory_msg_
ros message
std::vector< trajectory_msgs::JointTrajectoryPoint > points_
std::string robot_namespace_
for setting ROS name space
physics::LinkPtr reference_link_
pose should be set relative to this link (default to "world")
void SetTrajectory(const trajectory_msgs::JointTrajectory::ConstPtr &trajectory)
Update the controller.
ros::NodeHandle * rosnode_
pointer to ros node
std::vector< gazebo::physics::JointPtr > joints_
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the controller.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27