00001 00002 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00003 #include <geometry_msgs/TwistWithCovariance.h> 00004 #include <geometry_msgs/QuaternionStamped.h> 00005 #include <geometry_msgs/WrenchStamped.h> 00006 #include <geometry_msgs/TwistStamped.h> 00007 #include <geometry_msgs/Quaternion.h> 00008 #include <geometry_msgs/Pose2D.h> 00009 #include <geometry_msgs/Point32.h> 00010 #include <geometry_msgs/PointStamped.h> 00011 #include <geometry_msgs/Transform.h> 00012 #include <geometry_msgs/Wrench.h> 00013 #include <geometry_msgs/Pose.h> 00014 #include <geometry_msgs/PoseStamped.h> 00015 #include <geometry_msgs/Vector3.h> 00016 #include <geometry_msgs/Twist.h> 00017 #include <geometry_msgs/TwistWithCovarianceStamped.h> 00018 #include <geometry_msgs/PolygonStamped.h> 00019 #include <geometry_msgs/PoseArray.h> 00020 #include <geometry_msgs/TransformStamped.h> 00021 #include <geometry_msgs/Point.h> 00022 #include <geometry_msgs/Vector3Stamped.h> 00023 #include <geometry_msgs/Polygon.h> 00024 #include <geometry_msgs/PoseWithCovariance.h> 00025 00026 #include "ros_msg_transporter.hpp" 00027 #include "RosLib.hpp" 00028 #include <rtt/types/TransportPlugin.hpp> 00029 #include <rtt/types/TypekitPlugin.hpp> 00030 00031 namespace ros_integration { 00032 using namespace RTT; 00033 struct ROSgeometry_msgsPlugin 00034 : public types::TransportPlugin 00035 { 00036 bool registerTransport(std::string name, types::TypeInfo* ti) 00037 { 00038 if(name == "/geometry_msgs/PoseWithCovarianceStamped") 00039 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::PoseWithCovarianceStamped>()); 00040 if(name == "/geometry_msgs/TwistWithCovariance") 00041 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::TwistWithCovariance>()); 00042 if(name == "/geometry_msgs/QuaternionStamped") 00043 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::QuaternionStamped>()); 00044 if(name == "/geometry_msgs/WrenchStamped") 00045 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::WrenchStamped>()); 00046 if(name == "/geometry_msgs/TwistStamped") 00047 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::TwistStamped>()); 00048 if(name == "/geometry_msgs/Quaternion") 00049 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Quaternion>()); 00050 if(name == "/geometry_msgs/Pose2D") 00051 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Pose2D>()); 00052 if(name == "/geometry_msgs/Point32") 00053 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Point32>()); 00054 if(name == "/geometry_msgs/PointStamped") 00055 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::PointStamped>()); 00056 if(name == "/geometry_msgs/Transform") 00057 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Transform>()); 00058 if(name == "/geometry_msgs/Wrench") 00059 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Wrench>()); 00060 if(name == "/geometry_msgs/Pose") 00061 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Pose>()); 00062 if(name == "/geometry_msgs/PoseStamped") 00063 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::PoseStamped>()); 00064 if(name == "/geometry_msgs/Vector3") 00065 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Vector3>()); 00066 if(name == "/geometry_msgs/Twist") 00067 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Twist>()); 00068 if(name == "/geometry_msgs/TwistWithCovarianceStamped") 00069 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::TwistWithCovarianceStamped>()); 00070 if(name == "/geometry_msgs/PolygonStamped") 00071 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::PolygonStamped>()); 00072 if(name == "/geometry_msgs/PoseArray") 00073 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::PoseArray>()); 00074 if(name == "/geometry_msgs/TransformStamped") 00075 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::TransformStamped>()); 00076 if(name == "/geometry_msgs/Point") 00077 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Point>()); 00078 if(name == "/geometry_msgs/Vector3Stamped") 00079 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Vector3Stamped>()); 00080 if(name == "/geometry_msgs/Polygon") 00081 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::Polygon>()); 00082 if(name == "/geometry_msgs/PoseWithCovariance") 00083 return ti->addProtocol(ORO_ROS_PROTOCOL_ID,new RosMsgTransporter<geometry_msgs::PoseWithCovariance>()); 00084 00085 return false; 00086 } 00087 00088 std::string getTransportName() const { 00089 return "ros"; 00090 } 00091 00092 std::string getTypekitName() const { 00093 return std::string("ros-")+"geometry_msgs"; 00094 } 00095 std::string getName() const { 00096 return std::string("rtt-ros-") + "geometry_msgs" + "-transport"; 00097 } 00098 00099 }; 00100 } 00101 00102 ORO_TYPEKIT_PLUGIN( ros_integration::ROSgeometry_msgsPlugin )