Go to the documentation of this file.00001
00010 #ifndef ROS_MSG_CONVERTER_H
00011 #define ROS_MSG_CONVERTER_H
00012
00013 #include <trajectory_msgs/JointTrajectory.h>
00014 #include "r2_msgs/PoseTrajectory.h"
00015 #include <sensor_msgs/JointState.h>
00016 #include "r2_msgs/PoseState.h"
00017 #include "r2_msgs/JointControlDataArray.h"
00018 #include "r2_msgs/WrenchState.h"
00019 #include "r2_msgs/JointCommand.h"
00020 #include "r2_msgs/ToolFrame.h"
00021
00022 #include <kdl/jntarrayvel.hpp>
00023 #include <kdl/frameacc.hpp>
00024 #include <kdl/segment.hpp>
00025 #include <kdl/jntarrayacc.hpp>
00026
00027 namespace RosMsgConverter
00028 {
00029
00030 struct JointVel
00031 {
00032 double q;
00033 double qdot;
00034 };
00035
00036 struct JointAcc
00037 {
00038 double q;
00039 double qdot;
00040 double qdotdot;
00041 };
00042
00043 struct JointCommand
00044 {
00045 double desiredPosition;
00046 double desiredPositionVelocityLimit;
00047 double feedForwardTorque;
00048 double porportionalGain;
00049 double derivativeGain;
00050 double integralGain;
00051 double positionLoopTorqueLimit;
00052 double positionLoopWindupLimit;
00053 double torqueLoopVelocityLimit;
00054 };
00055
00056 struct ToolFrame
00057 {
00058 std::string name;
00059 std::string parent;
00060 KDL::Segment segment;
00061 };
00062
00063 void JointStateToJntArray(const sensor_msgs::JointState& jointState, const std::vector<std::string> jointNames, KDL::JntArray& jointArray);
00064
00065 void JointStateToJntArrayVel(const sensor_msgs::JointState& jointState, const std::vector<std::string> jointNames, KDL::JntArrayVel& jointArray);
00066
00068 void JointStateToJntArrayAcc(const sensor_msgs::JointState& jointState, const std::vector<std::string> jointNames, KDL::JntArrayAcc& jointArray);
00069
00070 void JointStateToJntMap(const sensor_msgs::JointState& jointState, std::map<std::string, double>& jointMap);
00071
00072 void JointStateToJointVelMap(const sensor_msgs::JointState& jointState, std::map<std::string, JointVel >& jointMap);
00073
00074 void JointStateToJointAccMap(const sensor_msgs::JointState& jointState, std::map<std::string, JointAcc>& jointMap);
00075
00076 void PoseStateToFrame(const r2_msgs::PoseState& poseState, const std::string& baseName,
00077 const std::string& tipName, KDL::Frame& frame);
00078
00079 void PoseStateToFrameVel(const r2_msgs::PoseState& poseState, const std::string& baseName,
00080 const std::string& tipName, KDL::FrameVel& frame);
00081
00082 void PoseStateToFrameAcc(const r2_msgs::PoseState& poseState, const std::string& baseName,
00083 const std::string& tipName, KDL::FrameAcc& frame);
00084
00085 void PoseStateToFrameAccMap(const r2_msgs::PoseState& poseState, const std::string& base,
00086 std::map<std::string, KDL::FrameAcc>& frameaccs);
00087
00088 void PoseStateToFrameVelMap(const r2_msgs::PoseState& poseState, const std::string& base,
00089 std::map<std::string, KDL::FrameVel>& framevels);
00090
00091 void PoseStateToFrameMap(const r2_msgs::PoseState& poseState, const std::string& base,
00092 std::map<std::string, KDL::Frame>& frames);
00093
00094 void JointControlDataToCoeffStateMap(const r2_msgs::JointControlDataArray& jointData,
00095 std::map<std::string, r2_msgs::JointControlCoeffState>& jointMap);
00096 KDL::Wrench wrenchMsgToWrench(const geometry_msgs::Wrench& wrenchMsg);
00097
00098 geometry_msgs::Wrench wrenchToWrenchMsg(const KDL::Wrench& wrench);
00099
00100 void JointCommandToJointCommandMap(const r2_msgs::JointCommand& jointCommand, std::map<std::string, JointCommand>& jointCommandMap);
00101
00102 void WrenchStateToWrenchMap(const r2_msgs::WrenchState& wrenchState, std::map<std::string, KDL::Wrench>& wrenchMap);
00103
00104 void ToolFrameToSegment(const r2_msgs::ToolFrame& toolFrame, std::vector<ToolFrame>& segmentList);
00105
00106 void ToolFrameToSegmentAdd(const r2_msgs::ToolFrame& toolFrame, std::vector<ToolFrame>& segmentList, std::vector<ToolFrame>& addSegmentList);
00107
00108 void ToolFrameToSegmentRemove(const r2_msgs::ToolFrame& toolFrame, std::vector<ToolFrame>& removeSegmentList);
00109
00110 }
00111
00112 #endif