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
00033
00034
00035 #include "MotoPlus.h"
00036 #include "ParameterExtraction.h"
00037 #include "CtrlGroup.h"
00038 #include "SimpleMessage.h"
00039 #include "Controller.h"
00040
00041
00042
00043
00044 int Ros_SimpleMsg_JointFeedback(CtrlGroup* ctrlGroup, SimpleMsg* sendMsg);
00045 int Ros_SimpleMsg_MotionReply(SimpleMsg* receiveMsg, int result, int subcode, SimpleMsg* replyMsg);
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 int Ros_SimpleMsg_JointFeedback(CtrlGroup* ctrlGroup, SimpleMsg* sendMsg)
00056 {
00057 int bRet;
00058 long pulsePos[MAX_PULSE_AXES];
00059
00060
00061 memset(sendMsg, 0x00, sizeof(SimpleMsg));
00062
00063
00064 sendMsg->prefix.length = sizeof(SmHeader) + sizeof(SmBodyJointFeedback);
00065
00066
00067 sendMsg->header.msgType = ROS_MSG_JOINT_FEEDBACK;
00068 sendMsg->header.commType = ROS_COMM_TOPIC;
00069 sendMsg->header.replyType = ROS_REPLY_INVALID;
00070
00071
00072 sendMsg->body.jointFeedback.groupNo = ctrlGroup->groupNo;
00073 sendMsg->body.jointFeedback.validFields = 2;
00074
00075 bRet = Ros_CtrlGroup_GetFBPulsePos(ctrlGroup, pulsePos);
00076 if(bRet!=TRUE)
00077 return 0;
00078
00079 Ros_CtrlGroup_ConvertToRosPos(ctrlGroup, pulsePos, sendMsg->body.jointFeedback.pos);
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089 return(sendMsg->prefix.length + sizeof(SmPrefix));
00090 }
00091
00092
00093
00094
00095
00096 int Ros_SimpleMsg_MotionReply(SimpleMsg* receiveMsg, int result, int subcode, SimpleMsg* replyMsg)
00097 {
00098
00099 memset(replyMsg, 0x00, sizeof(SimpleMsg));
00100
00101
00102 replyMsg->prefix.length = sizeof(SmHeader) + sizeof(SmBodyMotoMotionReply);
00103
00104
00105 replyMsg->header.msgType = ROS_MSG_MOTO_MOTION_REPLY;
00106 replyMsg->header.commType = ROS_COMM_SERVICE_REPLY;
00107 replyMsg->header.replyType = ROS_REPLY_SUCCESS;
00108
00109
00110 if(receiveMsg->header.msgType == ROS_MSG_MOTO_MOTION_CTRL)
00111 {
00112 replyMsg->body.motionReply.groupNo = receiveMsg->body.motionCtrl.groupNo;
00113 replyMsg->body.motionReply.sequence = receiveMsg->body.motionCtrl.sequence;
00114 replyMsg->body.motionReply.command = receiveMsg->body.motionCtrl.command;
00115 }
00116 else if(receiveMsg->header.msgType == ROS_MSG_JOINT_TRAJ_PT_FULL)
00117 {
00118 replyMsg->body.motionReply.groupNo = receiveMsg->body.jointTrajData.groupNo;
00119 replyMsg->body.motionReply.sequence = receiveMsg->body.jointTrajData.sequence;
00120 replyMsg->body.motionReply.command = ROS_MSG_JOINT_TRAJ_PT_FULL;
00121 }
00122 else
00123 {
00124 replyMsg->body.motionReply.groupNo = -1;
00125 replyMsg->body.motionReply.sequence = -1;
00126 replyMsg->body.motionReply.command = receiveMsg->header.msgType;
00127 }
00128
00129 replyMsg->body.motionReply.result = result;
00130 replyMsg->body.motionReply.subcode = subcode;
00131
00132 return(replyMsg->prefix.length + sizeof(SmPrefix));
00133 }
00134
00135 #ifdef DEBUG
00136
00137 void Ros_SimpleMsg_DumpTrajPtFull(SmBodyJointTrajPtFull* data)
00138 {
00139 printf("Dumping SmBodyJointTrajPtFull:\r\n");
00140 printf(" groupNo=%d\r\n", data->groupNo);
00141 printf(" sequence=%d\r\n", data->sequence);
00142 printf(" validFields=%d\r\n", data->validFields);
00143 printf(" time=%%.5f\r\n", data->time);
00144 printf(" pos: %.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\r\n",
00145 data->pos[0], data->pos[1], data->pos[2], data->pos[3],
00146 data->pos[4], data->pos[5], data->pos[6]);
00147 printf(" vel: %.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\r\n",
00148 data->vel[0], data->vel[1], data->vel[2], data->vel[3],
00149 data->vel[4], data->vel[5], data->vel[6]);
00150 printf(" acc: %.5f, %.5f, %.5f, %.5f, %.5f, %.5f, %.5f\r\n",
00151 data->acc[0], data->acc[1], data->acc[2], data->acc[3],
00152 data->acc[4], data->acc[5], data->acc[6]);
00153 }
00154 #endif