biped_serializer.cpp
Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 #include "std_msgs/UInt8MultiArray.h"
00003 #include "std_srvs/Empty.h"
00004 #include "visualization_msgs/MarkerArray.h"
00005 #include "visualization_msgs/Marker.h"
00006 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00007 #include "tf/transform_broadcaster.h"
00008 #include "tf/transform_listener.h"
00009 #include "biped_robin_msgs/StepTarget3DService.h"
00010 #include "biped_robin_msgs/SetModeService.h"
00011 #include "biped_robin_msgs/SetParamService.h"
00012 #include "biped_robin_msgs/MovePostureCartesianService.h"
00013 #include "biped_robin_msgs/MovePostureJointService.h"
00014 #include "biped_robin_msgs/MoveGripperService.h"
00015 
00016  /*
00017  * This node serializes the data to send to RTAI via mailbox
00018  */
00019 
00020 #define DEFAULT_VARIABLE_NUMBER 150
00021 #define DEFAULT_WORDLENGTH 40   /* multible of 4 and compatible with simulink receiving file */
00022 
00023 
00024 struct DataPoint
00025 {
00026         char name[DEFAULT_WORDLENGTH];
00027         long timestamp;
00028         double value;
00029 };
00030 
00031 struct DataPointArray
00032 {
00033         struct DataPoint dataPoint[DEFAULT_VARIABLE_NUMBER];
00034         int length;
00035 };
00036 
00037 ros::Publisher rtai_data;
00038 int old_left_right = 1;
00039 unsigned int stepCounter = 0;
00040 
00041 
00042 bool sendParameterToRobot (const char* name, double value) {
00043         struct DataPointArray data;
00044 
00045         ROS_INFO("Robot Parameter \"%s\" will be set to value: %f.", name, value);
00046 
00047         /* populate data */
00048         strcpy(data.dataPoint[0].name, name); data.dataPoint[0].value = value;
00049         /* set number of parameters */
00050         data.length = 1;
00051         /* set timestamps */
00052         for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
00053 
00054         //copy DataPointArray to vector
00055         std_msgs::UInt8MultiArray msgOut;
00056         //TODO: header to be added!
00057         for (unsigned int i=0; i<sizeof(DataPointArray); i++) {
00058                 msgOut.data.push_back(((char*)&data)[i]);
00059         }
00060         rtai_data.publish(msgOut);
00061 
00062         return true;
00063 }
00064 
00065 bool footstepIncCallback(biped_robin_msgs::StepTarget3DService::Request &req, biped_robin_msgs::StepTarget3DService::Response &res) {
00066         ROS_INFO("Passing incremental step to robot");  
00067 
00068         struct DataPointArray data;
00069         double roll=0;
00070         double pitch=0;
00071         double yaw=0;   
00072         
00073         // btMatrix3x3(btQuaternion(req.step.pose.orientation.x, req.step.pose.orientation.y, req.step.pose.orientation.z, req.step.pose.orientation.w)).getEulerYPR(yaw, pitch, roll);
00074         tf::Quaternion quat;    
00075         tf::quaternionMsgToTF(req.step.pose.orientation, quat);
00076         tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
00077 
00078         if(req.step.leg == 0){
00079                 strcpy(data.dataPoint[0].name, "srv_desiredStepInc_feet"); data.dataPoint[0].value = 2;
00080         } else {
00081                 strcpy(data.dataPoint[0].name, "srv_desiredStepInc_feet"); data.dataPoint[0].value = 1;
00082         }       
00083         strcpy(data.dataPoint[1].name, "srv_desiredStepInc_x"); data.dataPoint[1].value = req.step.pose.position.x;
00084         strcpy(data.dataPoint[2].name, "srv_desiredStepInc_y"); data.dataPoint[2].value = req.step.pose.position.y;
00085         strcpy(data.dataPoint[3].name, "srv_desiredStepInc_z"); data.dataPoint[3].value = req.step.pose.position.z;
00086         strcpy(data.dataPoint[4].name, "srv_desiredStepInc_alpha"); data.dataPoint[4].value = roll;
00087         strcpy(data.dataPoint[5].name, "srv_desiredStepInc_beta"); data.dataPoint[5].value = pitch;
00088         strcpy(data.dataPoint[6].name, "srv_desiredStepInc_gamma"); data.dataPoint[6].value = yaw;
00089         strcpy(data.dataPoint[7].name, "srv_executeStep"); data.dataPoint[7].value = ++stepCounter;
00090         ROS_INFO("This was Step Number %d", stepCounter);
00091 
00092         data.length = 8;
00093         /*TODO:  set timestamps */
00094         for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
00095         //copy DataPointArray to vector
00096         std_msgs::UInt8MultiArray msgOut;
00097         //TODO: header to be added!
00098         for (unsigned int i=0; i<sizeof(DataPointArray); i++) {
00099                 msgOut.data.push_back(((char*)&data)[i]);
00100         }
00101         rtai_data.publish(msgOut);
00102 
00103         res.successful = true;
00104 
00105         return true;
00106 }
00107 
00108 bool movePostureCartesianCallback(biped_robin_msgs::MovePostureCartesianService::Request &req, biped_robin_msgs::MovePostureCartesianService::Response &res) {
00109         ROS_INFO("Passing cartesian posture to robot");
00110         struct DataPointArray data;
00111         double roll=0;
00112         double pitch=0;
00113         double yaw=0;
00114 
00115         
00116         tf::Quaternion quat;    
00117         tf::quaternionMsgToTF(req.upper_body_posture, quat);
00118         tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
00119 
00120         strcpy(data.dataPoint[0].name, "srv_cartMoveTime"); data.dataPoint[0].value = req.time_to_move;
00121         strcpy(data.dataPoint[1].name, "srv_cartMoveTCPPos_x"); data.dataPoint[1].value = req.right_tcp_position.x;
00122         strcpy(data.dataPoint[2].name, "srv_cartMoveTCPPos_y"); data.dataPoint[2].value = req.right_tcp_position.y;
00123         strcpy(data.dataPoint[3].name, "srv_cartMoveTCPPos_z"); data.dataPoint[3].value = req.right_tcp_position.z;
00124         strcpy(data.dataPoint[4].name, "srv_cartMoveRB_alpha"); data.dataPoint[4].value = roll;
00125         strcpy(data.dataPoint[5].name, "srv_cartMoveRB_beta"); data.dataPoint[5].value = pitch;
00126         strcpy(data.dataPoint[6].name, "srv_cartMoveRB_gamma"); data.dataPoint[6].value = yaw;
00127         strcpy(data.dataPoint[7].name, "srv_cartMoveLeftArm"); data.dataPoint[7].value = req.left_arm_angle;
00128         strcpy(data.dataPoint[8].name, "srv_cartMoveRightArm"); data.dataPoint[8].value = req.right_arm_angle;
00129 
00130         data.length = 9;
00131         /*TODO:  set timestamps */
00132         for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
00133         //copy DataPointArray to vector
00134         std_msgs::UInt8MultiArray msgOut;
00135         //TODO: header to be added!
00136         for (unsigned int i=0; i<sizeof(DataPointArray); i++) {
00137                 msgOut.data.push_back(((char*)&data)[i]);
00138         }
00139         rtai_data.publish(msgOut);
00140 
00141         res.successful = true;
00142 
00143         return true;
00144 }
00145 
00146 bool movePostureJointCallback(biped_robin_msgs::MovePostureJointService::Request &req, biped_robin_msgs::MovePostureJointService::Response &res) {
00147         ROS_INFO("Passing joint posture to robot");
00148         struct DataPointArray data;
00149 
00150         strcpy(data.dataPoint[0].name, "srv_jointMoveTime"); data.dataPoint[0].value = req.time_to_move;
00151         strcpy(data.dataPoint[1].name, "srv_joint_q1"); data.dataPoint[1].value = req.joint_positions[0];
00152         strcpy(data.dataPoint[2].name, "srv_joint_q2"); data.dataPoint[2].value = req.joint_positions[1];
00153         strcpy(data.dataPoint[3].name, "srv_joint_q3"); data.dataPoint[3].value = req.joint_positions[2];
00154         strcpy(data.dataPoint[4].name, "srv_joint_q4"); data.dataPoint[4].value = req.joint_positions[3];
00155         strcpy(data.dataPoint[5].name, "srv_joint_q5"); data.dataPoint[5].value = req.joint_positions[4];
00156         strcpy(data.dataPoint[6].name, "srv_joint_q6"); data.dataPoint[6].value = req.joint_positions[5];
00157         strcpy(data.dataPoint[7].name, "srv_joint_q7"); data.dataPoint[7].value = req.joint_positions[6];
00158         strcpy(data.dataPoint[8].name, "srv_joint_q8"); data.dataPoint[8].value = req.joint_positions[7];
00159         strcpy(data.dataPoint[9].name, "srv_joint_q9"); data.dataPoint[9].value = req.joint_positions[8];
00160         strcpy(data.dataPoint[10].name, "srv_joint_q10"); data.dataPoint[10].value = req.joint_positions[9];
00161         strcpy(data.dataPoint[11].name, "srv_joint_q11"); data.dataPoint[11].value = req.joint_positions[10];
00162         strcpy(data.dataPoint[12].name, "srv_joint_q12"); data.dataPoint[12].value = req.joint_positions[11];
00163         strcpy(data.dataPoint[13].name, "srv_joint_q13"); data.dataPoint[13].value = req.joint_positions[12];
00164         strcpy(data.dataPoint[14].name, "srv_joint_q14"); data.dataPoint[14].value = req.joint_positions[13];
00165 
00166         data.length = 15;
00167         /*TODO:  set timestamps */
00168         for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
00169         //copy DataPointArray to vector
00170         std_msgs::UInt8MultiArray msgOut;
00171         //TODO: header to be added!
00172         for (unsigned int i=0; i<sizeof(DataPointArray); i++) {
00173                 msgOut.data.push_back(((char*)&data)[i]);
00174         }
00175         rtai_data.publish(msgOut);
00176 
00177         res.successful = true;
00178 
00179         return true;
00180 }
00181 
00182 bool moveGripperCallback(biped_robin_msgs::MoveGripperService::Request &req, biped_robin_msgs::MoveGripperService::Response &res) {
00183         ROS_INFO("Passing gripper speed to robot");
00184         res.successful = true;
00185         return sendParameterToRobot("srv_moveGripperSpeed", req.speed);
00186 }
00187 
00188 bool acknowledgeCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
00189         ROS_INFO("Acknowledge Service called");
00190         return sendParameterToRobot("srv_acknowledgement", rand()%100);
00191 }
00192 
00193 bool referenceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
00194         ROS_INFO("Reference Service called");
00195         return sendParameterToRobot("srv_referenceDrives", rand()%100);
00196 }
00197 
00198 bool stopCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
00199         ROS_INFO("Stop Service called");
00200         return sendParameterToRobot("srv_emergencyStop", rand()%100);
00201 }
00202 
00203 bool resetCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
00204         ROS_INFO("Reset Internal State Service called");
00205         return sendParameterToRobot("srv_resetInternalState", rand()%100);
00206 }
00207 
00208 bool setModeCallback(biped_robin_msgs::SetModeService::Request &req, biped_robin_msgs::SetModeService::Response &res) {
00209         ROS_INFO("Set Mode Service called. Desired Mode is set to %d", req.desiredMode);
00210         res.successful = true;
00211         return sendParameterToRobot("srv_setDesiredMotionMode", req.desiredMode);
00212 }
00213 
00214 bool setParamCallback(biped_robin_msgs::SetParamService::Request &req, biped_robin_msgs::SetParamService::Response &res) {
00215         ROS_INFO("Set Parameter Service called.");
00216         res.successful = true;
00217         return sendParameterToRobot(req.parameterName.c_str(), req.parameterValue);
00218 }
00219 
00220 
00221 int main(int argc, char **argv)
00222 {
00223   /* init ROS */
00224 
00225   ros::init(argc, argv, "biped_serializer");
00226   ros::NodeHandle n;
00227         ros::NodeHandle pn("~");
00228 
00229   rtai_data = n.advertise<std_msgs::UInt8MultiArray>("data_to_rtai", 1000);
00230   //rtai_data = pn.advertise<std_msgs::UInt8MultiArray>("unchangeableSteps", 10);
00231 
00232   ros::ServiceServer footstep3DInc_srv = n.advertiseService("footstep3DInc_srv", footstepIncCallback);
00233   ros::ServiceServer movePostureCartesian_srv = n.advertiseService("movePostureCartesian_srv", movePostureCartesianCallback);
00234   ros::ServiceServer movePostureJoint_srv = n.advertiseService("movePostureJoint_srv", movePostureJointCallback);
00235   ros::ServiceServer moveGripper_srv = n.advertiseService("moveGripper_srv", moveGripperCallback);
00236   ros::ServiceServer acknowledge_srv = n.advertiseService("acknowledge_srv", acknowledgeCallback);
00237   ros::ServiceServer reference_srv = n.advertiseService("reference_srv", referenceCallback);
00238   ros::ServiceServer stop_srv = n.advertiseService("stop_srv", stopCallback);
00239   ros::ServiceServer reset_srv = n.advertiseService("resetState_srv", resetCallback);
00240   ros::ServiceServer setMode_srv = n.advertiseService("setMode_srv", setModeCallback);
00241   ros::ServiceServer setParam_srv = n.advertiseService("setParam_srv", setParamCallback);
00242 
00243   ros::Duration(2.0).sleep();   
00244   ros::spinOnce();
00245 
00246   bool isReal;
00247   n.param("/par_isRealEnvironment", isReal, false);
00248   if (isReal) {
00249           ROS_INFO("The Robot is present in reality");
00250   } else {
00251           ROS_INFO("The Robot is just simulated");
00252   }
00253   sendParameterToRobot("par_isRealEnvironment", isReal);
00254 
00255   ros::spin();
00256 
00257   return 0;
00258 }
00259 
00260 


biped_robin_driver
Author(s): Johannes Mayr
autogenerated on Mon Jan 6 2014 11:07:54