Classes | |
struct | JointAcc |
struct | JointCommand |
struct | JointVel |
struct | ToolFrame |
Functions | |
void | JointCommandToJointCommandMap (const r2_msgs::JointCommand &jointCommand, std::map< std::string, JointCommand > &jointCommandMap) |
void | JointControlDataToCoeffStateMap (const r2_msgs::JointControlDataArray &jointData, std::map< std::string, r2_msgs::JointControlCoeffState > &jointMap) |
void | JointStateToJntArray (const sensor_msgs::JointState &jointState, const std::vector< std::string > jointNames, KDL::JntArray &jointArray) |
JointStateToJntArray. | |
void | JointStateToJntArrayAcc (const sensor_msgs::JointState &jointState, const std::vector< std::string > jointNames, KDL::JntArrayAcc &jointArray) |
Acc term contains effort ///. | |
void | JointStateToJntArrayVel (const sensor_msgs::JointState &jointState, const std::vector< std::string > jointNames, KDL::JntArrayVel &jointArray) |
JointStateToJntArrayVel. | |
void | JointStateToJntMap (const sensor_msgs::JointState &jointState, std::map< std::string, double > &jointMap) |
JointStateToJntMap creates a map of jointName and position. | |
void | JointStateToJointAccMap (const sensor_msgs::JointState &jointState, std::map< std::string, JointAcc > &jointMap) |
JointStateEffortToJntMap ToJntMap creates a map of jointName and effort. | |
void | JointStateToJointVelMap (const sensor_msgs::JointState &jointState, std::map< std::string, JointVel > &jointMap) |
JointStateVel ToJntMap creates a map of jointName and velocity. | |
void | PoseStateToFrame (const r2_msgs::PoseState &poseState, const std::string &baseName, const std::string &tipName, KDL::Frame &frame) |
PoseStateToFrame. | |
void | PoseStateToFrameAcc (const r2_msgs::PoseState &poseState, const std::string &baseName, const std::string &tipName, KDL::FrameAcc &frame) |
PoseStateToFrameAcc. | |
void | PoseStateToFrameAccMap (const r2_msgs::PoseState &poseState, const std::string &base, std::map< std::string, KDL::FrameAcc > &frameaccs) |
updates a map of all the frames in poseState with their new location and orientations | |
void | PoseStateToFrameMap (const r2_msgs::PoseState &poseState, const std::string &base, std::map< std::string, KDL::Frame > &frames) |
updates a map of all the frames in poseState with their new location and orientations | |
void | PoseStateToFrameVel (const r2_msgs::PoseState &poseState, const std::string &baseName, const std::string &tipName, KDL::FrameVel &frame) |
PoseStateToFrameVel. | |
void | PoseStateToFrameVelMap (const r2_msgs::PoseState &poseState, const std::string &base, std::map< std::string, KDL::FrameVel > &framevels) |
updates a map of all the frames in poseState with their new location and orientations | |
void | ToolFrameToSegment (const r2_msgs::ToolFrame &toolFrame, std::vector< ToolFrame > &segmentList) |
creates a segment map from a toolFrame message | |
void | ToolFrameToSegmentAdd (const r2_msgs::ToolFrame &toolFrame, std::vector< ToolFrame > &segmentList, std::vector< ToolFrame > &addSegmentList) |
void | ToolFrameToSegmentRemove (const r2_msgs::ToolFrame &toolFrame, std::vector< ToolFrame > &removeSegmentList) |
KDL::Wrench | wrenchMsgToWrench (const geometry_msgs::Wrench &wrenchMsg) |
void | WrenchStateToWrenchMap (const r2_msgs::WrenchState &wrenchState, std::map< std::string, KDL::Wrench > &wrenchMap) |
geometry_msgs::Wrench | wrenchToWrenchMsg (const KDL::Wrench &wrench) |
void RosMsgConverter::JointCommandToJointCommandMap | ( | const r2_msgs::JointCommand & | jointCommand, |
std::map< std::string, JointCommand > & | jointCommandMap | ||
) |
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only add them
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only add them
Definition at line 885 of file RosMsgConverter.cpp.
void RosMsgConverter::JointControlDataToCoeffStateMap | ( | const r2_msgs::JointControlDataArray & | jointData, |
std::map< std::string, r2_msgs::JointControlCoeffState > & | jointMap | ||
) |
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only addd them
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only addd them
Definition at line 829 of file RosMsgConverter.cpp.
void RosMsgConverter::JointStateToJntArray | ( | const sensor_msgs::JointState & | jointState, |
const std::vector< std::string > | jointNames, | ||
KDL::JntArray & | jointArray | ||
) |
JointStateToJntArray.
jointState | jointState of joints |
jointNames | names of joints to extract |
jointArray | array of joint position values extracted from jointState |
logic_error | jointState name length doesn't match positions length |
logic_error | jointState name length less than trajectory name length |
logic_error | jointState doesn't contain all joints in jointNames |
extracts joints from jointState based on jointNames. If jointNames and jointArray are the same size, resizing jointArray is avoided
Definition at line 21 of file RosMsgConverter.cpp.
void RosMsgConverter::JointStateToJntArrayAcc | ( | const sensor_msgs::JointState & | jointState, |
const std::vector< std::string > | jointNames, | ||
KDL::JntArrayAcc & | jointArray | ||
) |
Acc term contains effort ///.
Definition at line 166 of file RosMsgConverter.cpp.
void RosMsgConverter::JointStateToJntArrayVel | ( | const sensor_msgs::JointState & | jointState, |
const std::vector< std::string > | jointNames, | ||
KDL::JntArrayVel & | jointArray | ||
) |
JointStateToJntArrayVel.
jointState | jointState of joints |
jointNames | names of joints to extract |
jointArray | array of joint values extracted from jointState |
logic_error | jointState name length doesn't match positions length |
logic_error | jointState name length less than trajectory name length |
logic_error | jointState doesn't contain all joints in jointNames |
extracts joints from jointState based on jointNames. If jointNames and jointArray are the same size, resizing jointArray is avoided
Definition at line 86 of file RosMsgConverter.cpp.
void RosMsgConverter::JointStateToJntMap | ( | const sensor_msgs::JointState & | jointState, |
std::map< std::string, double > & | jointMap | ||
) |
JointStateToJntMap creates a map of jointName and position.
jointState | the jointState message |
jointMap | the output map of jointName and position |
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only add them
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only add them
Definition at line 668 of file RosMsgConverter.cpp.
void RosMsgConverter::JointStateToJointAccMap | ( | const sensor_msgs::JointState & | jointState, |
std::map< std::string, JointAcc > & | jointMap | ||
) |
JointStateEffortToJntMap ToJntMap creates a map of jointName and effort.
jointState | the jointState message |
jointMap | the output map of jointName and effort |
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only addd them
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only addd them
Definition at line 766 of file RosMsgConverter.cpp.
void RosMsgConverter::JointStateToJointVelMap | ( | const sensor_msgs::JointState & | jointState, |
std::map< std::string, JointVel > & | jointMap | ||
) |
JointStateVel ToJntMap creates a map of jointName and velocity.
jointState | the jointState message |
jointMap | the output map of jointName and velocity |
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only addd them
if jointmap is empty, add all elements
otherwise, locate those elements in jointMap and only addd them
Definition at line 709 of file RosMsgConverter.cpp.
void RosMsgConverter::PoseStateToFrame | ( | const r2_msgs::PoseState & | poseState, |
const std::string & | baseName, | ||
const std::string & | tipName, | ||
KDL::Frame & | frame | ||
) |
PoseStateToFrame.
poseState | poseState of robot |
baseFrame | name of the reference frame for returned pose |
tipFrame | name of the tip frame to return |
frame | returned pose |
logic_error | poseState name length doesn't match pose length |
logic_error | poseState doesn't contain the necessary transform information |
extracts poses from poseState and performs necessary transformations
Definition at line 248 of file RosMsgConverter.cpp.
void RosMsgConverter::PoseStateToFrameAcc | ( | const r2_msgs::PoseState & | poseState, |
const std::string & | baseName, | ||
const std::string & | tipName, | ||
KDL::FrameAcc & | frame | ||
) |
PoseStateToFrameAcc.
poseState | poseState of robot |
baseFrame | name of the reference frame for returned pose |
tipFrame | name of the tip frame to return |
frame | returned pose |
logic_error | poseState name length doesn't match pose length |
logic_error | poseState doesn't contain the necessary transform information |
extracts poses from poseState and performs necessary transformations
Definition at line 438 of file RosMsgConverter.cpp.
void RosMsgConverter::PoseStateToFrameAccMap | ( | const r2_msgs::PoseState & | poseState, |
const std::string & | base, | ||
std::map< std::string, KDL::FrameAcc > & | frameaccs | ||
) |
updates a map of all the frames in poseState with their new location and orientations
poseState | the poseState calculated from forward kinematics |
base | the base frame from which to calculate all the poses |
frameaccs | the output frame map |
Definition at line 632 of file RosMsgConverter.cpp.
void RosMsgConverter::PoseStateToFrameMap | ( | const r2_msgs::PoseState & | poseState, |
const std::string & | base, | ||
std::map< std::string, KDL::Frame > & | frames | ||
) |
updates a map of all the frames in poseState with their new location and orientations
poseState | the poseState calculated from forward kinematics |
base | the base frame from which to calculate all the poses |
frames | the output frame map |
Definition at line 558 of file RosMsgConverter.cpp.
void RosMsgConverter::PoseStateToFrameVel | ( | const r2_msgs::PoseState & | poseState, |
const std::string & | baseName, | ||
const std::string & | tipName, | ||
KDL::FrameVel & | frame | ||
) |
PoseStateToFrameVel.
poseState | poseState of robot |
baseFrame | name of the reference frame for returned pose |
tipFrame | name of the tip frame to return |
frame | returned pose |
logic_error | poseState name length doesn't match pose length |
logic_error | poseState doesn't contain the necessary transform information |
extracts poses from poseState and performs necessary transformations
Definition at line 329 of file RosMsgConverter.cpp.
void RosMsgConverter::PoseStateToFrameVelMap | ( | const r2_msgs::PoseState & | poseState, |
const std::string & | base, | ||
std::map< std::string, KDL::FrameVel > & | framevels | ||
) |
updates a map of all the frames in poseState with their new location and orientations
poseState | the poseState calculated from forward kinematics |
base | the base frame from which to calculate all the poses |
framevels | the output frame map |
Definition at line 595 of file RosMsgConverter.cpp.
void RosMsgConverter::ToolFrameToSegment | ( | const r2_msgs::ToolFrame & | toolFrame, |
std::vector< ToolFrame > & | segmentList | ||
) |
creates a segment map from a toolFrame message
toolFrame | The input toolFrame message |
segmentMap | The output segmentMap hook_name: KDL::Segment |
Definition at line 1050 of file RosMsgConverter.cpp.
void RosMsgConverter::ToolFrameToSegmentAdd | ( | const r2_msgs::ToolFrame & | toolFrame, |
std::vector< ToolFrame > & | segmentList, | ||
std::vector< ToolFrame > & | addSegmentList | ||
) |
Definition at line 1077 of file RosMsgConverter.cpp.
void RosMsgConverter::ToolFrameToSegmentRemove | ( | const r2_msgs::ToolFrame & | toolFrame, |
std::vector< ToolFrame > & | removeSegmentList | ||
) |
Definition at line 1105 of file RosMsgConverter.cpp.
KDL::Wrench RosMsgConverter::wrenchMsgToWrench | ( | const geometry_msgs::Wrench & | wrenchMsg | ) |
Definition at line 867 of file RosMsgConverter.cpp.
void RosMsgConverter::WrenchStateToWrenchMap | ( | const r2_msgs::WrenchState & | wrenchState, |
std::map< std::string, KDL::Wrench > & | wrenchMap | ||
) |
Definition at line 1028 of file RosMsgConverter.cpp.
geometry_msgs::Wrench RosMsgConverter::wrenchToWrenchMsg | ( | const KDL::Wrench & | wrench | ) |
Definition at line 873 of file RosMsgConverter.cpp.