biped_robin_driver: biped_deserializer.cpp Source File

Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 #include "nav_msgs/Odometry.h"
00003 #include "std_msgs/UInt8MultiArray.h"
00004 #include "tf/transform_broadcaster.h"
00005 #include "std_msgs/Float64.h"
00006 #include "math.h"
00007 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00008 #include "std_msgs/UInt8.h"
00009 #include "sensor_msgs/JointState.h"
00010 #include "biped_robin_msgs/StepTarget3D.h"
00011 #include "biped_robin_msgs/BipedRobinGlobalBehaviour.h"
00012 
00013  /*
00014  * This node deserializes the data received from RTAI via mailbox
00015  */
00016 
00017 #define DEFAULT_VARIABLE_NUMBER 150
00018 #define DEFAULT_WORDLENGTH 40   /* multible of 4 and compatible with simulink receiving file */
00019 #define PI 3.14159265
00020 
00021 struct DataPoint
00022 {
00023         char name[DEFAULT_WORDLENGTH];
00024         long timestamp;
00025         double value;
00026 };
00027 
00028 struct DataPointArray
00029 {
00030         struct DataPoint dataPoint[DEFAULT_VARIABLE_NUMBER];
00031         int length;
00032 };
00033 
00034 ros::Publisher currentStep_pub;
00035 ros::Publisher globalBehaviour_pub;
00036 ros::Publisher joint_pub;
00037 ros::Publisher stepsLeftInBuffer_pub;
00038 double oldTime = 0;
00039 
00040 
00041 void rtaiMsgCallback(const std_msgs::UInt8MultiArray rtaiData) {
00042         static tf::TransformBroadcaster brcom;
00043         static tf::TransformBroadcaster brbody;
00044         static tf::TransformBroadcaster brhead;
00045         static tf::TransformBroadcaster brleft;
00046         static tf::TransformBroadcaster brright;
00047         static tf::TransformBroadcaster brbase;
00048         static tf::TransformBroadcaster brcenter;
00049 
00050         struct DataPointArray data;
00051         copy(rtaiData.data.begin(), rtaiData.data.end(), (char*)&data);
00052         double q_Vector[15];
00053 
00054         //parse to twist topic from data stream
00055         std_msgs::Float64 test;
00056         tf::Vector3 com_Vector;
00057         tf::Vector3 body_Vector;
00058         tf::Vector3 head_Vector;
00059         tf::Vector3 left_Vector;
00060         tf::Vector3 right_Vector;
00061         tf::Vector3 center_Vector;
00062         tf::Vector3 map_Vector;
00063         tf::Transform com_Transform;
00064         tf::Transform body_Transform;
00065         tf::Transform head_Transform;
00066         tf::Transform left_Transform;
00067         tf::Transform right_Transform;
00068         tf::Transform center_Transform;
00069         tf::Transform map_Transform;
00070         std_msgs::UInt8 stepsLeftInBuffer;
00071         sensor_msgs::JointState joint_state;
00072         biped_robin_msgs::StepTarget3D currentStep;
00073         biped_robin_msgs::BipedRobinGlobalBehaviour globalBehaviour;
00074 
00075         double com_Matrix_values[9];
00076         double body_Matrix_values[9];
00077         double head_Matrix_values[9];
00078         double left_Matrix_values[9];
00079         double right_Matrix_values[9];
00080         double currentStep_values[7];
00081         double globalBehaviour_values[9];
00082         int stepsLeftInBuffer_value = 0;
00083 
00084         bool sendCoMTransform=false;
00085         bool sendLeftTransform=false;
00086         bool sendRightTransform=false;
00087         bool sendBodyTransform=false;
00088         bool sendHeadTransform=false;
00089         bool sendJointState=false;
00090         bool sendCurrentStep=false;
00091         bool sendStepsLeftInBuffer=false;
00092         bool sendGlobalBehaviour=false;
00093 
00094         //if (ros::Time::now().toSec()-oldTime > 0.1){  // uncomment to reduce frequency of posting stuff
00095                 for (int i=0; i<data.length; i++) {
00096                   if (strcmp(data.dataPoint[i].name, "")) {
00097                         /* fill out headers!!!! */
00098                         /* put in right names for the different values */
00099                         if (!strcmp(data.dataPoint[i].name, "comx")) {com_Vector.setX(data.dataPoint[i].value); sendCoMTransform=true;}
00100                         if (!strcmp(data.dataPoint[i].name, "comy")) com_Vector.setY(data.dataPoint[i].value);
00101                         if (!strcmp(data.dataPoint[i].name, "comz")) com_Vector.setZ(data.dataPoint[i].value);
00102                         if (!strcmp(data.dataPoint[i].name, "rb1")) com_Matrix_values[0] = data.dataPoint[i].value;
00103                         if (!strcmp(data.dataPoint[i].name, "rb2")) com_Matrix_values[1] = data.dataPoint[i].value;
00104                         if (!strcmp(data.dataPoint[i].name, "rb3")) com_Matrix_values[2] = data.dataPoint[i].value;
00105                         if (!strcmp(data.dataPoint[i].name, "rb4")) com_Matrix_values[3] = data.dataPoint[i].value;
00106                         if (!strcmp(data.dataPoint[i].name, "rb5")) com_Matrix_values[4] = data.dataPoint[i].value;
00107                         if (!strcmp(data.dataPoint[i].name, "rb6")) com_Matrix_values[5] = data.dataPoint[i].value;
00108                         if (!strcmp(data.dataPoint[i].name, "rb7")) com_Matrix_values[6] = data.dataPoint[i].value;
00109                         if (!strcmp(data.dataPoint[i].name, "rb8")) com_Matrix_values[7] = data.dataPoint[i].value;
00110                         if (!strcmp(data.dataPoint[i].name, "rb9")) com_Matrix_values[8] = data.dataPoint[i].value;
00111                         if (!strcmp(data.dataPoint[i].name, "tl1")) {left_Matrix_values[0] = data.dataPoint[i].value; sendLeftTransform=true;}
00112                         if (!strcmp(data.dataPoint[i].name, "tl2")) left_Matrix_values[1] = data.dataPoint[i].value;
00113                         if (!strcmp(data.dataPoint[i].name, "tl3")) left_Matrix_values[2] = data.dataPoint[i].value;
00114                         if (!strcmp(data.dataPoint[i].name, "tl4")) left_Matrix_values[3] = data.dataPoint[i].value;
00115                         if (!strcmp(data.dataPoint[i].name, "tl5")) left_Matrix_values[4] = data.dataPoint[i].value;
00116                         if (!strcmp(data.dataPoint[i].name, "tl6")) left_Matrix_values[5] = data.dataPoint[i].value;
00117                         if (!strcmp(data.dataPoint[i].name, "tl7")) left_Matrix_values[6] = data.dataPoint[i].value;
00118                         if (!strcmp(data.dataPoint[i].name, "tl8")) left_Matrix_values[7] = data.dataPoint[i].value;
00119                         if (!strcmp(data.dataPoint[i].name, "tl9")) left_Matrix_values[8] = data.dataPoint[i].value;
00120                         if (!strcmp(data.dataPoint[i].name, "tl10")) left_Vector.setX(data.dataPoint[i].value);
00121                         if (!strcmp(data.dataPoint[i].name, "tl11")) left_Vector.setY(data.dataPoint[i].value);
00122                         if (!strcmp(data.dataPoint[i].name, "tl12")) left_Vector.setZ(data.dataPoint[i].value);
00123                         if (!strcmp(data.dataPoint[i].name, "tr1")) {right_Matrix_values[0] = data.dataPoint[i].value; sendRightTransform=true;}
00124                         if (!strcmp(data.dataPoint[i].name, "tr2")) right_Matrix_values[1] = data.dataPoint[i].value;
00125                         if (!strcmp(data.dataPoint[i].name, "tr3")) right_Matrix_values[2] = data.dataPoint[i].value;
00126                         if (!strcmp(data.dataPoint[i].name, "tr4")) right_Matrix_values[3] = data.dataPoint[i].value;
00127                         if (!strcmp(data.dataPoint[i].name, "tr5")) right_Matrix_values[4] = data.dataPoint[i].value;
00128                         if (!strcmp(data.dataPoint[i].name, "tr6")) right_Matrix_values[5] = data.dataPoint[i].value;
00129                         if (!strcmp(data.dataPoint[i].name, "tr7")) right_Matrix_values[6] = data.dataPoint[i].value;
00130                         if (!strcmp(data.dataPoint[i].name, "tr8")) right_Matrix_values[7] = data.dataPoint[i].value;
00131                         if (!strcmp(data.dataPoint[i].name, "tr9")) right_Matrix_values[8] = data.dataPoint[i].value;
00132                         if (!strcmp(data.dataPoint[i].name, "tr10")) right_Vector.setX(data.dataPoint[i].value);
00133                         if (!strcmp(data.dataPoint[i].name, "tr11")) right_Vector.setY(data.dataPoint[i].value);
00134                         if (!strcmp(data.dataPoint[i].name, "tr12")) right_Vector.setZ(data.dataPoint[i].value);
00135                         if (!strcmp(data.dataPoint[i].name, "tb1")) {body_Matrix_values[0] = data.dataPoint[i].value; sendBodyTransform=true;}
00136                         if (!strcmp(data.dataPoint[i].name, "tb2")) body_Matrix_values[1] = data.dataPoint[i].value;
00137                         if (!strcmp(data.dataPoint[i].name, "tb3")) body_Matrix_values[2] = data.dataPoint[i].value;
00138                         if (!strcmp(data.dataPoint[i].name, "tb4")) body_Matrix_values[3] = data.dataPoint[i].value;
00139                         if (!strcmp(data.dataPoint[i].name, "tb5")) body_Matrix_values[4] = data.dataPoint[i].value;
00140                         if (!strcmp(data.dataPoint[i].name, "tb6")) body_Matrix_values[5] = data.dataPoint[i].value;
00141                         if (!strcmp(data.dataPoint[i].name, "tb7")) body_Matrix_values[6] = data.dataPoint[i].value;
00142                         if (!strcmp(data.dataPoint[i].name, "tb8")) body_Matrix_values[7] = data.dataPoint[i].value;
00143                         if (!strcmp(data.dataPoint[i].name, "tb9")) body_Matrix_values[8] = data.dataPoint[i].value;
00144                         if (!strcmp(data.dataPoint[i].name, "tb10")) body_Vector.setX(data.dataPoint[i].value);
00145                         if (!strcmp(data.dataPoint[i].name, "tb11")) body_Vector.setY(data.dataPoint[i].value);
00146                         if (!strcmp(data.dataPoint[i].name, "tb12")) body_Vector.setZ(data.dataPoint[i].value);
00147                         if (!strcmp(data.dataPoint[i].name, "th1")) {head_Matrix_values[0] = data.dataPoint[i].value; sendHeadTransform=true;}
00148                         if (!strcmp(data.dataPoint[i].name, "th2")) head_Matrix_values[1] = data.dataPoint[i].value;
00149                         if (!strcmp(data.dataPoint[i].name, "th3")) head_Matrix_values[2] = data.dataPoint[i].value;
00150                         if (!strcmp(data.dataPoint[i].name, "th4")) head_Matrix_values[3] = data.dataPoint[i].value;
00151                         if (!strcmp(data.dataPoint[i].name, "th5")) head_Matrix_values[4] = data.dataPoint[i].value;
00152                         if (!strcmp(data.dataPoint[i].name, "th6")) head_Matrix_values[5] = data.dataPoint[i].value;
00153                         if (!strcmp(data.dataPoint[i].name, "th7")) head_Matrix_values[6] = data.dataPoint[i].value;
00154                         if (!strcmp(data.dataPoint[i].name, "th8")) head_Matrix_values[7] = data.dataPoint[i].value;
00155                         if (!strcmp(data.dataPoint[i].name, "th9")) head_Matrix_values[8] = data.dataPoint[i].value;
00156                         if (!strcmp(data.dataPoint[i].name, "th10")) head_Vector.setX(data.dataPoint[i].value);
00157                         if (!strcmp(data.dataPoint[i].name, "th11")) head_Vector.setY(data.dataPoint[i].value);
00158                         if (!strcmp(data.dataPoint[i].name, "th12")) head_Vector.setZ(data.dataPoint[i].value);
00159                         if (!strcmp(data.dataPoint[i].name, "q1")) {q_Vector[1] = data.dataPoint[i].value; sendJointState=true;}
00160                         if (!strcmp(data.dataPoint[i].name, "q2")) q_Vector[2] = data.dataPoint[i].value;
00161                         if (!strcmp(data.dataPoint[i].name, "q3")) q_Vector[3] = data.dataPoint[i].value;
00162                         if (!strcmp(data.dataPoint[i].name, "q4")) q_Vector[4] = data.dataPoint[i].value;
00163                         if (!strcmp(data.dataPoint[i].name, "q5")) q_Vector[5] = data.dataPoint[i].value;
00164                         if (!strcmp(data.dataPoint[i].name, "q6")) q_Vector[6] = data.dataPoint[i].value;
00165                         if (!strcmp(data.dataPoint[i].name, "q7")) q_Vector[7] = data.dataPoint[i].value;
00166                         if (!strcmp(data.dataPoint[i].name, "q8")) q_Vector[8] = data.dataPoint[i].value;
00167                         if (!strcmp(data.dataPoint[i].name, "q9")) q_Vector[9] = data.dataPoint[i].value;
00168                         if (!strcmp(data.dataPoint[i].name, "q10")) q_Vector[10] = data.dataPoint[i].value;
00169                         if (!strcmp(data.dataPoint[i].name, "q11")) q_Vector[11] = data.dataPoint[i].value;
00170                         if (!strcmp(data.dataPoint[i].name, "q12")) q_Vector[12] = data.dataPoint[i].value;
00171                         if (!strcmp(data.dataPoint[i].name, "q13")) q_Vector[13] = data.dataPoint[i].value;
00172                         if (!strcmp(data.dataPoint[i].name, "q14")) q_Vector[14] = data.dataPoint[i].value;
00173                         if (!strcmp(data.dataPoint[i].name, "res_currentStep_feet")) {currentStep_values[0] = data.dataPoint[i].value; sendCurrentStep=true;}
00174                         if (!strcmp(data.dataPoint[i].name, "res_currentStep_z")) currentStep_values[1] = data.dataPoint[i].value;
00175                         if (!strcmp(data.dataPoint[i].name, "res_currentStep_y")) currentStep_values[2] = data.dataPoint[i].value;
00176                         if (!strcmp(data.dataPoint[i].name, "res_currentStep_x")) currentStep_values[3] = data.dataPoint[i].value;
00177                         if (!strcmp(data.dataPoint[i].name, "res_currentStep_alpha")) currentStep_values[4] = data.dataPoint[i].value;
00178                         if (!strcmp(data.dataPoint[i].name, "res_currentStep_beta"))  currentStep_values[5] = data.dataPoint[i].value;
00179                         if (!strcmp(data.dataPoint[i].name, "res_currentStep_gamma")) currentStep_values[6] = data.dataPoint[i].value;
00180                         if (!strcmp(data.dataPoint[i].name, "res_stepsLeftInBuffer")) {stepsLeftInBuffer_value = data.dataPoint[i].value; sendStepsLeftInBuffer=true;}
00181                         if (!strcmp(data.dataPoint[i].name, "globBehav_areJointErrors")) {globalBehaviour_values[0] = data.dataPoint[i].value; sendGlobalBehaviour=true;}
00182                         if (!strcmp(data.dataPoint[i].name, "globBehav_areJointsReferenced")) globalBehaviour_values[1] = data.dataPoint[i].value;
00183                         if (!strcmp(data.dataPoint[i].name, "globBehav_areJointTowErrorsSmall")) globalBehaviour_values[2] = data.dataPoint[i].value;
00184                         if (!strcmp(data.dataPoint[i].name, "globBehav_areJointsReadyToDrive")) globalBehaviour_values[3] = data.dataPoint[i].value;
00185                         if (!strcmp(data.dataPoint[i].name, "globBehav_areForceSensorsOK"))  globalBehaviour_values[4] = data.dataPoint[i].value;
00186                         if (!strcmp(data.dataPoint[i].name, "globBehav_isAirborne")) globalBehaviour_values[5] = data.dataPoint[i].value;
00187                         if (!strcmp(data.dataPoint[i].name, "globBehav_areJointsReadyToDriveAir")) globalBehaviour_values[6] = data.dataPoint[i].value;
00188                         if (!strcmp(data.dataPoint[i].name, "globBehav_isReadyToWalk")) globalBehaviour_values[7] = data.dataPoint[i].value;
00189                         if (!strcmp(data.dataPoint[i].name, "globBehav_currentMotionMode")) globalBehaviour_values[8] = data.dataPoint[i].value;
00190                   }
00191                 }
00192 
00193                 ros::Time now = ros::Time::now();
00194 
00195                 if (sendBodyTransform) {
00196                         body_Transform = tf::Transform(tf::Matrix3x3(body_Matrix_values[0], body_Matrix_values[3],body_Matrix_values[6],body_Matrix_values[1],body_Matrix_values[4],body_Matrix_values[7],body_Matrix_values[2],body_Matrix_values[5],body_Matrix_values[8]), body_Vector);
00197                         brbody.sendTransform(tf::StampedTransform(body_Transform, now, "odom", "base_link"));
00198                 }
00199 
00200                 if (sendCoMTransform) {
00201                         com_Transform = tf::Transform(tf::Matrix3x3(com_Matrix_values[0], com_Matrix_values[3],com_Matrix_values[6],com_Matrix_values[1],com_Matrix_values[4],com_Matrix_values[7],com_Matrix_values[2],com_Matrix_values[5],com_Matrix_values[8]), com_Vector);
00202                         com_Transform.mult(body_Transform.inverse(), com_Transform);
00203                         brcom.sendTransform(tf::StampedTransform(com_Transform, now, "base_link", "center_of_mass"));
00204                 }
00205 
00206                 if (sendHeadTransform) {
00207                         head_Transform = tf::Transform(tf::Matrix3x3(head_Matrix_values[0], head_Matrix_values[3],head_Matrix_values[6],head_Matrix_values[1],head_Matrix_values[4],head_Matrix_values[7],head_Matrix_values[2],head_Matrix_values[5],head_Matrix_values[8]), head_Vector);
00208                         head_Transform.mult(body_Transform.inverse(), head_Transform);
00209                         brhead.sendTransform(tf::StampedTransform(head_Transform, now, "base_link", "gaze"));
00210                 }
00211 
00212                 if (sendLeftTransform) {
00213                         left_Transform = tf::Transform(tf::Matrix3x3(left_Matrix_values[0], left_Matrix_values[3],left_Matrix_values  [6],left_Matrix_values[1],left_Matrix_values[4],left_Matrix_values[7],left_Matrix_values[2],left_Matrix_values[5],left_Matrix_values[8]), left_Vector);
00214                         left_Transform.mult(body_Transform.inverse(), left_Transform);
00215                         brleft.sendTransform(tf::StampedTransform(left_Transform, now, "base_link", "l_sole"));
00216                 }
00217 
00218                 if (sendRightTransform) {
00219                         right_Transform = tf::Transform(tf::Matrix3x3(right_Matrix_values[0], right_Matrix_values[3],right_Matrix_values[6],right_Matrix_values[1],right_Matrix_values[4],right_Matrix_values[7],right_Matrix_values[2],right_Matrix_values[5],right_Matrix_values[8]), right_Vector);
00220                         right_Transform.mult(body_Transform.inverse(), right_Transform);
00221                         brright.sendTransform(tf::StampedTransform(right_Transform, now, "base_link", "r_sole"));
00222                 }
00223 
00224                 if (sendLeftTransform && sendRightTransform) {
00225                         double centerX = (right_Transform.getOrigin().getX()+left_Transform.getOrigin().getX())/2;
00226                         double centerY = (right_Transform.getOrigin().getY()+left_Transform.getOrigin().getY())/2;
00227                         double centerZ = fmin(left_Transform.getOrigin().getZ(), right_Transform.getOrigin().getZ());
00228 
00229                         center_Vector = tf::Vector3(centerX,centerY,centerZ);
00230                         center_Transform = tf::Transform(tf::Matrix3x3(1,0,0,0,1,0,0,0,1), center_Vector);
00231                         brcenter.sendTransform(tf::StampedTransform(center_Transform, now, "base_link", "base_footprint"));
00232                 }
00233 
00234                 if (sendCurrentStep) {
00235                         if (currentStep_values[0]==1) {
00236                                 currentStep.leg=1;
00237                         } else {
00238                                 currentStep.leg=0;
00239                         }
00240                         currentStep.pose.position.x = currentStep_values[1];
00241                         currentStep.pose.position.y = currentStep_values[2];
00242                         currentStep.pose.position.z = currentStep_values[3];
00243 
00244                         tf::Quaternion quatStep;
00245                         quatStep.setEuler(currentStep_values[6],currentStep_values[5],currentStep_values[4]);
00246 
00247                         currentStep.pose.orientation.x = quatStep.getX();
00248                         currentStep.pose.orientation.y = quatStep.getY();
00249                         currentStep.pose.orientation.z = quatStep.getZ();
00250                         currentStep.pose.orientation.w = quatStep.getW();
00251                         currentStep_pub.publish(currentStep);
00252                 }
00253 
00254                 if (sendJointState) {
00255                         joint_state.header.stamp = now;
00256                         joint_state.name.resize(12);
00257                         joint_state.position.resize(12);
00258                         joint_state.name[0] = "body_to_left_hip";
00259                         joint_state.position[0] = q_Vector[7];
00260                         joint_state.name[1] = "body_to_right_hip";
00261                         joint_state.position[1] = q_Vector[1];
00262                         joint_state.name[2] = "left_hip2";
00263                         joint_state.position[2] = q_Vector[8];
00264                         joint_state.name[3] = "right_hip2";
00265                         joint_state.position[3] = q_Vector[2];
00266                         joint_state.name[4] = "left_hip3";
00267                         joint_state.position[4] = q_Vector[9];
00268                         joint_state.name[5] = "right_hip3";
00269                         joint_state.position[5] = q_Vector[3];
00270                         joint_state.name[6] = "left_knee";
00271                         joint_state.position[6] = q_Vector[10];
00272                         joint_state.name[7] = "right_knee";
00273                         joint_state.position[7] = q_Vector[4];
00274                         joint_state.name[8] = "left_ankle";
00275                         joint_state.position[8] = q_Vector[11];
00276                         joint_state.name[9] = "right_ankle";
00277                         joint_state.position[9] = q_Vector[5];
00278                         joint_state.name[10] = "left_ankle2";
00279                         joint_state.position[10] = q_Vector[12];
00280                         joint_state.name[11] = "right_ankle2";
00281                         joint_state.position[11] = q_Vector[6];
00282                         // TODO add arms
00283 
00284                         joint_pub.publish(joint_state);
00285                 }
00286 
00287                 if (sendStepsLeftInBuffer) {
00288                         stepsLeftInBuffer.data = stepsLeftInBuffer_value;
00289                         stepsLeftInBuffer_pub.publish(stepsLeftInBuffer);
00290                 }
00291 
00292                 if (sendGlobalBehaviour) {
00293                         globalBehaviour.areJointErrors = (bool) globalBehaviour_values[0];
00294                         globalBehaviour.areJointsReferenced = (bool) globalBehaviour_values[1];
00295                         globalBehaviour.areJointTowErrorsSmall = (bool) globalBehaviour_values[2];
00296                         globalBehaviour.areJointsReadyToDrive = (bool) globalBehaviour_values[3];
00297                         globalBehaviour.areForceSensorsOK = (bool) globalBehaviour_values[4];
00298                         globalBehaviour.isAirborne = (bool) globalBehaviour_values[5];
00299                         globalBehaviour.areJointsReadyToDriveAirborne = (bool) globalBehaviour_values[6];
00300                         globalBehaviour.isReadyToWalk = (bool) globalBehaviour_values[7];
00301                         globalBehaviour.currentMotionMode = (char) globalBehaviour_values[8];
00302                         globalBehaviour_pub.publish(globalBehaviour);
00303                 }
00304 
00305                 oldTime = now.toSec();
00306         //}
00307 }
00308 
00309 
00310 
00311 int main(int argc, char **argv)
00312 {
00313         /* init ROS */
00314         ros::init(argc, argv, "biped_deserializer");
00315         ros::NodeHandle n;
00316         ros::Subscriber rtai_data = n.subscribe<std_msgs::UInt8MultiArray>("data_from_rtai", 1000, rtaiMsgCallback);
00317         joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
00318         currentStep_pub = n.advertise<biped_robin_msgs::StepTarget3D>("currentStep", 1);
00319         globalBehaviour_pub = n.advertise<biped_robin_msgs::BipedRobinGlobalBehaviour>("bipedRobinGlobalBehaviour", 1);
00320         stepsLeftInBuffer_pub = n.advertise<std_msgs::UInt8>("stepsLeftInBuffer", 1);
00321 
00322         ros::spin();
00323 
00324         return 0;
00325 }
00326 
00327