Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifdef ROS
00033 #include "dx100/ros_conversion.h"
00034 #include "simple_message/log_wrapper.h"
00035 using std::min;
00036 #define MAX_PULSE_AXES 8 // since MotoPlus.h is not availble in ROS
00037 #endif
00038
00039 #ifdef MOTOPLUS
00040 #include "ros_conversion.h"
00041 #include "log_wrapper.h"
00042 #endif
00043
00044 using namespace industrial::joint_data;
00045
00046 namespace motoman
00047 {
00048
00049 namespace ros_conversion
00050 {
00051
00052 void toRosJoint(float* mp_joints, JointData & ros_joints)
00053 {
00054
00055 JointData temp;
00056 int minJoints = min(MAX_PULSE_AXES, ros_joints.getMaxNumJoints());
00057 for (int i=0; i<minJoints; ++i)
00058 temp.setJoint(i, mp_joints[i]);
00059
00060
00061 ros_joints.copyFrom(temp);
00062 ros_joints.setJoint(RosJointIndexes::S, temp.getJoint(MotomanJointIndexes::S));
00063 ros_joints.setJoint(RosJointIndexes::L, temp.getJoint(MotomanJointIndexes::L));
00064 ros_joints.setJoint(RosJointIndexes::U, temp.getJoint(MotomanJointIndexes::U));
00065 ros_joints.setJoint(RosJointIndexes::R, temp.getJoint(MotomanJointIndexes::R));
00066 ros_joints.setJoint(RosJointIndexes::B, temp.getJoint(MotomanJointIndexes::B));
00067 ros_joints.setJoint(RosJointIndexes::T, temp.getJoint(MotomanJointIndexes::T));
00068 ros_joints.setJoint(RosJointIndexes::E, temp.getJoint(MotomanJointIndexes::E));
00069 }
00070
00071 void toMpJoint(industrial::joint_data::JointData & ros_joints,
00072 float* mp_joints)
00073 {
00074
00075 JointData temp;
00076 temp.copyFrom(ros_joints);
00077 temp.setJoint(MotomanJointIndexes::S, ros_joints.getJoint(RosJointIndexes::S));
00078 temp.setJoint(MotomanJointIndexes::L, ros_joints.getJoint(RosJointIndexes::L));
00079 temp.setJoint(MotomanJointIndexes::U, ros_joints.getJoint(RosJointIndexes::U));
00080 temp.setJoint(MotomanJointIndexes::R, ros_joints.getJoint(RosJointIndexes::R));
00081 temp.setJoint(MotomanJointIndexes::B, ros_joints.getJoint(RosJointIndexes::B));
00082 temp.setJoint(MotomanJointIndexes::T, ros_joints.getJoint(RosJointIndexes::T));
00083 temp.setJoint(MotomanJointIndexes::E, ros_joints.getJoint(RosJointIndexes::E));
00084
00085
00086 memset(mp_joints, 0, MAX_PULSE_AXES*sizeof(float));
00087 int minJoints = min(MAX_PULSE_AXES, ros_joints.getMaxNumJoints());
00088 for (int i=0; i<minJoints; ++i)
00089 mp_joints[i] = temp.getJoint(i);
00090 }
00091
00092
00093 void toRosJointOrder(JointData & joints)
00094 {
00095
00096 JointData swap;
00097 swap.setJoint(RosJointIndexes::S, joints.getJoint(MotomanJointIndexes::S));
00098 swap.setJoint(RosJointIndexes::L, joints.getJoint(MotomanJointIndexes::L));
00099 swap.setJoint(RosJointIndexes::U, joints.getJoint(MotomanJointIndexes::U));
00100 swap.setJoint(RosJointIndexes::R, joints.getJoint(MotomanJointIndexes::R));
00101 swap.setJoint(RosJointIndexes::B, joints.getJoint(MotomanJointIndexes::B));
00102 swap.setJoint(RosJointIndexes::T, joints.getJoint(MotomanJointIndexes::T));
00103 swap.setJoint(RosJointIndexes::E, joints.getJoint(MotomanJointIndexes::E));
00104 joints.copyFrom(swap);
00105 }
00106
00107 void toMotomanJointOrder(JointData & joints)
00108 {
00109
00110 JointData swap;
00111 swap.setJoint(MotomanJointIndexes::S, joints.getJoint(RosJointIndexes::S));
00112 swap.setJoint(MotomanJointIndexes::L, joints.getJoint(RosJointIndexes::L));
00113 swap.setJoint(MotomanJointIndexes::U, joints.getJoint(RosJointIndexes::U));
00114 swap.setJoint(MotomanJointIndexes::R, joints.getJoint(RosJointIndexes::R));
00115 swap.setJoint(MotomanJointIndexes::B, joints.getJoint(RosJointIndexes::B));
00116 swap.setJoint(MotomanJointIndexes::T, joints.getJoint(RosJointIndexes::T));
00117 swap.setJoint(MotomanJointIndexes::E, joints.getJoint(RosJointIndexes::E));
00118 joints.copyFrom(swap);
00119 }
00120
00121 }
00122 }