00001
00002 #ifndef GAZEBO_PLUGINS_MESSAGE_WORLDSTATE_H
00003 #define GAZEBO_PLUGINS_MESSAGE_WORLDSTATE_H
00004 #include <string>
00005 #include <vector>
00006 #include <ostream>
00007 #include "ros/serialization.h"
00008 #include "ros/builtin_message_traits.h"
00009 #include "ros/message_operations.h"
00010 #include "ros/message.h"
00011 #include "ros/time.h"
00012
00013 #include "std_msgs/Header.h"
00014 #include "geometry_msgs/Pose.h"
00015 #include "geometry_msgs/Twist.h"
00016 #include "geometry_msgs/Wrench.h"
00017
00018 namespace gazebo_plugins
00019 {
00020 template <class ContainerAllocator>
00021 struct WorldState_ : public ros::Message
00022 {
00023 typedef WorldState_<ContainerAllocator> Type;
00024
00025 WorldState_()
00026 : header()
00027 , name()
00028 , pose()
00029 , twist()
00030 , wrench()
00031 {
00032 }
00033
00034 WorldState_(const ContainerAllocator& _alloc)
00035 : header(_alloc)
00036 , name(_alloc)
00037 , pose(_alloc)
00038 , twist(_alloc)
00039 , wrench(_alloc)
00040 {
00041 }
00042
00043 typedef ::std_msgs::Header_<ContainerAllocator> _header_type;
00044 ::std_msgs::Header_<ContainerAllocator> header;
00045
00046 typedef std::vector<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > , typename ContainerAllocator::template rebind<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::other > _name_type;
00047 std::vector<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > , typename ContainerAllocator::template rebind<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::other > name;
00048
00049 typedef std::vector< ::geometry_msgs::Pose_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Pose_<ContainerAllocator> >::other > _pose_type;
00050 std::vector< ::geometry_msgs::Pose_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Pose_<ContainerAllocator> >::other > pose;
00051
00052 typedef std::vector< ::geometry_msgs::Twist_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Twist_<ContainerAllocator> >::other > _twist_type;
00053 std::vector< ::geometry_msgs::Twist_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Twist_<ContainerAllocator> >::other > twist;
00054
00055 typedef std::vector< ::geometry_msgs::Wrench_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Wrench_<ContainerAllocator> >::other > _wrench_type;
00056 std::vector< ::geometry_msgs::Wrench_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Wrench_<ContainerAllocator> >::other > wrench;
00057
00058
00059 ROS_DEPRECATED uint32_t get_name_size() const { return (uint32_t)name.size(); }
00060 ROS_DEPRECATED void set_name_size(uint32_t size) { name.resize((size_t)size); }
00061 ROS_DEPRECATED void get_name_vec(std::vector<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > , typename ContainerAllocator::template rebind<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::other > & vec) const { vec = this->name; }
00062 ROS_DEPRECATED void set_name_vec(const std::vector<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > , typename ContainerAllocator::template rebind<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::other > & vec) { this->name = vec; }
00063 ROS_DEPRECATED uint32_t get_pose_size() const { return (uint32_t)pose.size(); }
00064 ROS_DEPRECATED void set_pose_size(uint32_t size) { pose.resize((size_t)size); }
00065 ROS_DEPRECATED void get_pose_vec(std::vector< ::geometry_msgs::Pose_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Pose_<ContainerAllocator> >::other > & vec) const { vec = this->pose; }
00066 ROS_DEPRECATED void set_pose_vec(const std::vector< ::geometry_msgs::Pose_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Pose_<ContainerAllocator> >::other > & vec) { this->pose = vec; }
00067 ROS_DEPRECATED uint32_t get_twist_size() const { return (uint32_t)twist.size(); }
00068 ROS_DEPRECATED void set_twist_size(uint32_t size) { twist.resize((size_t)size); }
00069 ROS_DEPRECATED void get_twist_vec(std::vector< ::geometry_msgs::Twist_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Twist_<ContainerAllocator> >::other > & vec) const { vec = this->twist; }
00070 ROS_DEPRECATED void set_twist_vec(const std::vector< ::geometry_msgs::Twist_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Twist_<ContainerAllocator> >::other > & vec) { this->twist = vec; }
00071 ROS_DEPRECATED uint32_t get_wrench_size() const { return (uint32_t)wrench.size(); }
00072 ROS_DEPRECATED void set_wrench_size(uint32_t size) { wrench.resize((size_t)size); }
00073 ROS_DEPRECATED void get_wrench_vec(std::vector< ::geometry_msgs::Wrench_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Wrench_<ContainerAllocator> >::other > & vec) const { vec = this->wrench; }
00074 ROS_DEPRECATED void set_wrench_vec(const std::vector< ::geometry_msgs::Wrench_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::geometry_msgs::Wrench_<ContainerAllocator> >::other > & vec) { this->wrench = vec; }
00075 private:
00076 static const char* __s_getDataType_() { return "gazebo_plugins/WorldState"; }
00077 public:
00078 ROS_DEPRECATED static const std::string __s_getDataType() { return __s_getDataType_(); }
00079
00080 ROS_DEPRECATED const std::string __getDataType() const { return __s_getDataType_(); }
00081
00082 private:
00083 static const char* __s_getMD5Sum_() { return "de1a9de3ab7ba97ac0e9ec01a4eb481e"; }
00084 public:
00085 ROS_DEPRECATED static const std::string __s_getMD5Sum() { return __s_getMD5Sum_(); }
00086
00087 ROS_DEPRECATED const std::string __getMD5Sum() const { return __s_getMD5Sum_(); }
00088
00089 private:
00090 static const char* __s_getMessageDefinition_() { return "# This is a message that holds data necessary to reconstruct a snapshot of the world\n\
00091 #\n\
00092 # = Approach to Message Passing =\n\
00093 # The state of the world is defined by either\n\
00094 # 1. Inertial Model pose, twist\n\
00095 # * kinematic data - connectivity graph from Model to each Link\n\
00096 # * joint angles\n\
00097 # * joint velocities\n\
00098 # * Applied forces - Body wrench\n\
00099 # * relative transform from Body to each collision Geom\n\
00100 # Or\n\
00101 # 2. Inertial (absolute) Body pose, twist, wrench\n\
00102 # * relative transform from Body to each collision Geom - constant, so not sent over wire\n\
00103 # * back compute from canonical body info to get Model pose and twist.\n\
00104 #\n\
00105 # Chooing (2.) because it matches most physics engines out there\n\
00106 # and is simpler.\n\
00107 #\n\
00108 # = Future =\n\
00109 # Consider impacts on using reduced coordinates / graph (parent/child links) approach\n\
00110 # constraint and physics solvers.\n\
00111 #\n\
00112 # = Application =\n\
00113 # This message is used to do the following:\n\
00114 # * reconstruct the world and objects for sensor generation\n\
00115 # * stop / start simulation - need pose, twist, wrench of each body\n\
00116 # * collision detection - need pose of each collision geometry. velocity/acceleration if \n\
00117 #\n\
00118 # = Assumptions =\n\
00119 # Assuming that each (physics) processor node locally already has\n\
00120 # * collision information - Trimesh for Geoms, etc\n\
00121 # * relative transforms from Body to Geom - this is assumed to be fixed, do not send oved wire\n\
00122 # * inertial information - does not vary in time\n\
00123 # * visual information - does not vary in time\n\
00124 #\n\
00125 \n\
00126 Header header\n\
00127 \n\
00128 string[] name\n\
00129 geometry_msgs/Pose[] pose\n\
00130 geometry_msgs/Twist[] twist\n\
00131 geometry_msgs/Wrench[] wrench\n\
00132 \n\
00133 ================================================================================\n\
00134 MSG: std_msgs/Header\n\
00135 # Standard metadata for higher-level stamped data types.\n\
00136 # This is generally used to communicate timestamped data \n\
00137 # in a particular coordinate frame.\n\
00138 # \n\
00139 # sequence ID: consecutively increasing ID \n\
00140 uint32 seq\n\
00141 #Two-integer timestamp that is expressed as:\n\
00142 # * stamp.secs: seconds (stamp_secs) since epoch\n\
00143 # * stamp.nsecs: nanoseconds since stamp_secs\n\
00144 # time-handling sugar is provided by the client library\n\
00145 time stamp\n\
00146 #Frame this data is associated with\n\
00147 # 0: no frame\n\
00148 # 1: global frame\n\
00149 string frame_id\n\
00150 \n\
00151 ================================================================================\n\
00152 MSG: geometry_msgs/Pose\n\
00153 # A representation of pose in free space, composed of postion and orientation. \n\
00154 Point position\n\
00155 Quaternion orientation\n\
00156 \n\
00157 ================================================================================\n\
00158 MSG: geometry_msgs/Point\n\
00159 # This contains the position of a point in free space\n\
00160 float64 x\n\
00161 float64 y\n\
00162 float64 z\n\
00163 \n\
00164 ================================================================================\n\
00165 MSG: geometry_msgs/Quaternion\n\
00166 # This represents an orientation in free space in quaternion form.\n\
00167 \n\
00168 float64 x\n\
00169 float64 y\n\
00170 float64 z\n\
00171 float64 w\n\
00172 \n\
00173 ================================================================================\n\
00174 MSG: geometry_msgs/Twist\n\
00175 # This expresses velocity in free space broken into it's linear and angular parts. \n\
00176 Vector3 linear\n\
00177 Vector3 angular\n\
00178 \n\
00179 ================================================================================\n\
00180 MSG: geometry_msgs/Vector3\n\
00181 # This represents a vector in free space. \n\
00182 \n\
00183 float64 x\n\
00184 float64 y\n\
00185 float64 z\n\
00186 ================================================================================\n\
00187 MSG: geometry_msgs/Wrench\n\
00188 # This represents force in free space, seperated into \n\
00189 # it's linear and angular parts. \n\
00190 Vector3 force\n\
00191 Vector3 torque\n\
00192 \n\
00193 "; }
00194 public:
00195 ROS_DEPRECATED static const std::string __s_getMessageDefinition() { return __s_getMessageDefinition_(); }
00196
00197 ROS_DEPRECATED const std::string __getMessageDefinition() const { return __s_getMessageDefinition_(); }
00198
00199 ROS_DEPRECATED virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const
00200 {
00201 ros::serialization::OStream stream(write_ptr, 1000000000);
00202 ros::serialization::serialize(stream, header);
00203 ros::serialization::serialize(stream, name);
00204 ros::serialization::serialize(stream, pose);
00205 ros::serialization::serialize(stream, twist);
00206 ros::serialization::serialize(stream, wrench);
00207 return stream.getData();
00208 }
00209
00210 ROS_DEPRECATED virtual uint8_t *deserialize(uint8_t *read_ptr)
00211 {
00212 ros::serialization::IStream stream(read_ptr, 1000000000);
00213 ros::serialization::deserialize(stream, header);
00214 ros::serialization::deserialize(stream, name);
00215 ros::serialization::deserialize(stream, pose);
00216 ros::serialization::deserialize(stream, twist);
00217 ros::serialization::deserialize(stream, wrench);
00218 return stream.getData();
00219 }
00220
00221 ROS_DEPRECATED virtual uint32_t serializationLength() const
00222 {
00223 uint32_t size = 0;
00224 size += ros::serialization::serializationLength(header);
00225 size += ros::serialization::serializationLength(name);
00226 size += ros::serialization::serializationLength(pose);
00227 size += ros::serialization::serializationLength(twist);
00228 size += ros::serialization::serializationLength(wrench);
00229 return size;
00230 }
00231
00232 typedef boost::shared_ptr< ::gazebo_plugins::WorldState_<ContainerAllocator> > Ptr;
00233 typedef boost::shared_ptr< ::gazebo_plugins::WorldState_<ContainerAllocator> const> ConstPtr;
00234 };
00235 typedef ::gazebo_plugins::WorldState_<std::allocator<void> > WorldState;
00236
00237 typedef boost::shared_ptr< ::gazebo_plugins::WorldState> WorldStatePtr;
00238 typedef boost::shared_ptr< ::gazebo_plugins::WorldState const> WorldStateConstPtr;
00239
00240
00241 template<typename ContainerAllocator>
00242 std::ostream& operator<<(std::ostream& s, const ::gazebo_plugins::WorldState_<ContainerAllocator> & v)
00243 {
00244 ros::message_operations::Printer< ::gazebo_plugins::WorldState_<ContainerAllocator> >::stream(s, "", v);
00245 return s;}
00246
00247 }
00248
00249 namespace ros
00250 {
00251 namespace message_traits
00252 {
00253 template<class ContainerAllocator>
00254 struct MD5Sum< ::gazebo_plugins::WorldState_<ContainerAllocator> > {
00255 static const char* value()
00256 {
00257 return "de1a9de3ab7ba97ac0e9ec01a4eb481e";
00258 }
00259
00260 static const char* value(const ::gazebo_plugins::WorldState_<ContainerAllocator> &) { return value(); }
00261 static const uint64_t static_value1 = 0xde1a9de3ab7ba97aULL;
00262 static const uint64_t static_value2 = 0xc0e9ec01a4eb481eULL;
00263 };
00264
00265 template<class ContainerAllocator>
00266 struct DataType< ::gazebo_plugins::WorldState_<ContainerAllocator> > {
00267 static const char* value()
00268 {
00269 return "gazebo_plugins/WorldState";
00270 }
00271
00272 static const char* value(const ::gazebo_plugins::WorldState_<ContainerAllocator> &) { return value(); }
00273 };
00274
00275 template<class ContainerAllocator>
00276 struct Definition< ::gazebo_plugins::WorldState_<ContainerAllocator> > {
00277 static const char* value()
00278 {
00279 return "# This is a message that holds data necessary to reconstruct a snapshot of the world\n\
00280 #\n\
00281 # = Approach to Message Passing =\n\
00282 # The state of the world is defined by either\n\
00283 # 1. Inertial Model pose, twist\n\
00284 # * kinematic data - connectivity graph from Model to each Link\n\
00285 # * joint angles\n\
00286 # * joint velocities\n\
00287 # * Applied forces - Body wrench\n\
00288 # * relative transform from Body to each collision Geom\n\
00289 # Or\n\
00290 # 2. Inertial (absolute) Body pose, twist, wrench\n\
00291 # * relative transform from Body to each collision Geom - constant, so not sent over wire\n\
00292 # * back compute from canonical body info to get Model pose and twist.\n\
00293 #\n\
00294 # Chooing (2.) because it matches most physics engines out there\n\
00295 # and is simpler.\n\
00296 #\n\
00297 # = Future =\n\
00298 # Consider impacts on using reduced coordinates / graph (parent/child links) approach\n\
00299 # constraint and physics solvers.\n\
00300 #\n\
00301 # = Application =\n\
00302 # This message is used to do the following:\n\
00303 # * reconstruct the world and objects for sensor generation\n\
00304 # * stop / start simulation - need pose, twist, wrench of each body\n\
00305 # * collision detection - need pose of each collision geometry. velocity/acceleration if \n\
00306 #\n\
00307 # = Assumptions =\n\
00308 # Assuming that each (physics) processor node locally already has\n\
00309 # * collision information - Trimesh for Geoms, etc\n\
00310 # * relative transforms from Body to Geom - this is assumed to be fixed, do not send oved wire\n\
00311 # * inertial information - does not vary in time\n\
00312 # * visual information - does not vary in time\n\
00313 #\n\
00314 \n\
00315 Header header\n\
00316 \n\
00317 string[] name\n\
00318 geometry_msgs/Pose[] pose\n\
00319 geometry_msgs/Twist[] twist\n\
00320 geometry_msgs/Wrench[] wrench\n\
00321 \n\
00322 ================================================================================\n\
00323 MSG: std_msgs/Header\n\
00324 # Standard metadata for higher-level stamped data types.\n\
00325 # This is generally used to communicate timestamped data \n\
00326 # in a particular coordinate frame.\n\
00327 # \n\
00328 # sequence ID: consecutively increasing ID \n\
00329 uint32 seq\n\
00330 #Two-integer timestamp that is expressed as:\n\
00331 # * stamp.secs: seconds (stamp_secs) since epoch\n\
00332 # * stamp.nsecs: nanoseconds since stamp_secs\n\
00333 # time-handling sugar is provided by the client library\n\
00334 time stamp\n\
00335 #Frame this data is associated with\n\
00336 # 0: no frame\n\
00337 # 1: global frame\n\
00338 string frame_id\n\
00339 \n\
00340 ================================================================================\n\
00341 MSG: geometry_msgs/Pose\n\
00342 # A representation of pose in free space, composed of postion and orientation. \n\
00343 Point position\n\
00344 Quaternion orientation\n\
00345 \n\
00346 ================================================================================\n\
00347 MSG: geometry_msgs/Point\n\
00348 # This contains the position of a point in free space\n\
00349 float64 x\n\
00350 float64 y\n\
00351 float64 z\n\
00352 \n\
00353 ================================================================================\n\
00354 MSG: geometry_msgs/Quaternion\n\
00355 # This represents an orientation in free space in quaternion form.\n\
00356 \n\
00357 float64 x\n\
00358 float64 y\n\
00359 float64 z\n\
00360 float64 w\n\
00361 \n\
00362 ================================================================================\n\
00363 MSG: geometry_msgs/Twist\n\
00364 # This expresses velocity in free space broken into it's linear and angular parts. \n\
00365 Vector3 linear\n\
00366 Vector3 angular\n\
00367 \n\
00368 ================================================================================\n\
00369 MSG: geometry_msgs/Vector3\n\
00370 # This represents a vector in free space. \n\
00371 \n\
00372 float64 x\n\
00373 float64 y\n\
00374 float64 z\n\
00375 ================================================================================\n\
00376 MSG: geometry_msgs/Wrench\n\
00377 # This represents force in free space, seperated into \n\
00378 # it's linear and angular parts. \n\
00379 Vector3 force\n\
00380 Vector3 torque\n\
00381 \n\
00382 ";
00383 }
00384
00385 static const char* value(const ::gazebo_plugins::WorldState_<ContainerAllocator> &) { return value(); }
00386 };
00387
00388 template<class ContainerAllocator> struct HasHeader< ::gazebo_plugins::WorldState_<ContainerAllocator> > : public TrueType {};
00389 template<class ContainerAllocator> struct HasHeader< const ::gazebo_plugins::WorldState_<ContainerAllocator> > : public TrueType {};
00390 }
00391 }
00392
00393 namespace ros
00394 {
00395 namespace serialization
00396 {
00397
00398 template<class ContainerAllocator> struct Serializer< ::gazebo_plugins::WorldState_<ContainerAllocator> >
00399 {
00400 template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
00401 {
00402 stream.next(m.header);
00403 stream.next(m.name);
00404 stream.next(m.pose);
00405 stream.next(m.twist);
00406 stream.next(m.wrench);
00407 }
00408
00409 ROS_DECLARE_ALLINONE_SERIALIZER;
00410 };
00411 }
00412 }
00413
00414 namespace ros
00415 {
00416 namespace message_operations
00417 {
00418
00419 template<class ContainerAllocator>
00420 struct Printer< ::gazebo_plugins::WorldState_<ContainerAllocator> >
00421 {
00422 template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::gazebo_plugins::WorldState_<ContainerAllocator> & v)
00423 {
00424 s << indent << "header: ";
00425 s << std::endl;
00426 Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + " ", v.header);
00427 s << indent << "name[]" << std::endl;
00428 for (size_t i = 0; i < v.name.size(); ++i)
00429 {
00430 s << indent << " name[" << i << "]: ";
00431 Printer<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::stream(s, indent + " ", v.name[i]);
00432 }
00433 s << indent << "pose[]" << std::endl;
00434 for (size_t i = 0; i < v.pose.size(); ++i)
00435 {
00436 s << indent << " pose[" << i << "]: ";
00437 s << std::endl;
00438 s << indent;
00439 Printer< ::geometry_msgs::Pose_<ContainerAllocator> >::stream(s, indent + " ", v.pose[i]);
00440 }
00441 s << indent << "twist[]" << std::endl;
00442 for (size_t i = 0; i < v.twist.size(); ++i)
00443 {
00444 s << indent << " twist[" << i << "]: ";
00445 s << std::endl;
00446 s << indent;
00447 Printer< ::geometry_msgs::Twist_<ContainerAllocator> >::stream(s, indent + " ", v.twist[i]);
00448 }
00449 s << indent << "wrench[]" << std::endl;
00450 for (size_t i = 0; i < v.wrench.size(); ++i)
00451 {
00452 s << indent << " wrench[" << i << "]: ";
00453 s << std::endl;
00454 s << indent;
00455 Printer< ::geometry_msgs::Wrench_<ContainerAllocator> >::stream(s, indent + " ", v.wrench[i]);
00456 }
00457 }
00458 };
00459
00460
00461 }
00462 }
00463
00464 #endif // GAZEBO_PLUGINS_MESSAGE_WORLDSTATE_H
00465