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
00018
00019
00020 #define DEFAULT_VARIABLE_NUMBER 150
00021 #define DEFAULT_WORDLENGTH 40
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
00048 strcpy(data.dataPoint[0].name, name); data.dataPoint[0].value = value;
00049
00050 data.length = 1;
00051
00052 for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
00053
00054
00055 std_msgs::UInt8MultiArray msgOut;
00056
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
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
00094 for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
00095
00096 std_msgs::UInt8MultiArray msgOut;
00097
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
00132 for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
00133
00134 std_msgs::UInt8MultiArray msgOut;
00135
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
00168 for (int i=0; i<data.length; i++) data.dataPoint[i].timestamp = 0xffff;
00169
00170 std_msgs::UInt8MultiArray msgOut;
00171
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
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
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