28 #include <sys/types.h>
34 #include <tf/transform_datatypes.h>
36 #include <boost/assign/list_of.hpp>
53 px += ((this->
xpos/1e3) * cos(rot) -
54 (this->
ypos/1e3) * sin(rot));
55 py += ((this->
xpos/1e3) * sin(rot) +
56 (this->
ypos/1e3) * cos(rot));
61 px += this->
xpos / 1e3;
62 py += this->
ypos / 1e3;
67 data->
position.header.frame_id =
"odom";
68 data->
position.child_frame_id =
"base_link";
70 data->
position.pose.pose.position.x = px;
71 data->
position.pose.pose.position.y = py;
72 data->
position.pose.pose.position.z = 0.0;
73 data->
position.pose.pose.orientation = tf::createQuaternionMsgFromYaw(pa);
77 data->
position.twist.twist.linear.y = 0.0;
81 data->
position.pose.covariance = boost::assign::list_of (1e-3) (0) (0) (0) (0) (0)
82 (0) (1e-3) (0) (0) (0) (0)
83 (0) (0) (1e6) (0) (0) (0)
84 (0) (0) (0) (1e6) (0) (0)
85 (0) (0) (0) (0) (1e6) (0)
86 (0) (0) (0) (0) (0) (1e3) ;
88 data->
position.twist.covariance = boost::assign::list_of (1e-3) (0) (0) (0) (0) (0)
89 (0) (1e-3) (0) (0) (0) (0)
90 (0) (0) (1e6) (0) (0) (0)
91 (0) (0) (0) (1e6) (0) (0)
92 (0) (0) (0) (0) (1e6) (0)
93 (0) (0) (0) (0) (0) (1e3) ;
100 data->
odom_trans.transform.translation.y = py;
102 data->
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pa);
122 data->
sonar.ranges.clear();
123 for(
int i=0; i < data->
sonar.ranges_count; i++)
128 unsigned char gripState =
timer;
129 if ((gripState & 0x01) && (gripState & 0x02) && !(gripState & 0x04))
134 else if(gripState & 0x04)
142 else if(gripState & 0x01)
147 else if(gripState & 0x02)
154 data->
gripper.grip.inner_beam =
false;
155 data->
gripper.grip.outer_beam =
false;
156 data->
gripper.grip.left_contact =
false;
157 data->
gripper.grip.right_contact =
false;
161 data->
gripper.grip.inner_beam =
true;
165 data->
gripper.grip.outer_beam =
true;
169 data->
gripper.grip.left_contact =
true;
173 data->
gripper.grip.right_contact =
true;
179 if ((gripState & 0x10) && (gripState & 0x20) && !(gripState & 0x40))
186 else if (gripState & 0x40)
192 if (gripState & 0x10)
194 else if (gripState & 0x20)
197 else if (gripState & 0x10)
200 data->
gripper.lift.position = 1.0f;
203 else if (gripState & 0x20)
206 data->
gripper.lift.position = 0.0f;
238 data->
dio.bits = (
unsigned int)this->
digin;
243 data->
aio.voltages_count = (
unsigned char)1;
247 data->
aio.voltages.clear();
248 data->
aio.voltages.push_back((this->
analog / 255.0) * 5.0);
258 diff2 = - ( from + 4096 - to );
262 diff2 = 4096 - from + to;
265 if ( abs(diff1) < abs(diff2) )
278 std::stringstream front_bumper_info;
279 for(
int i=0;i<5;i++) {
280 front_bumper_info <<
" "
283 ROS_DEBUG(
"Front bumpers:%s", front_bumper_info.str().c_str());
284 std::stringstream rear_bumper_info;
285 for(
int i=0;i<5;i++) {
286 rear_bumper_info <<
" "
289 ROS_DEBUG(
"Rear bumpers:%s", rear_bumper_info.str().c_str());
292 std::stringstream status_info;
295 <<
static_cast<int>((
status >> (7-i) ) & 0x01);
297 ROS_DEBUG(
"status:%s", status_info.str().c_str());
298 std::stringstream digin_info;
301 <<
static_cast<int>((
digin >> (7-i) ) & 0x01);
303 ROS_DEBUG(
"digin:%s", digin_info.str().c_str());
304 std::stringstream digout_info;
307 <<
static_cast<int>((
digout >> (7-i) ) & 0x01);
309 ROS_DEBUG(
"digout:%s", digout_info.str().c_str());
310 ROS_DEBUG(
"battery: %d compass: %d sonarreadings: %d\n",
battery,
313 ROS_DEBUG(
"angle: %d lvel: %d rvel: %d control: %d\n",
angle,
lvel,
325 std::stringstream sonar_info;
327 sonar_info <<
" " <<
static_cast<int>(
sonars[i]);
329 ROS_DEBUG(
"Sonars: %s", sonar_info.str().c_str());
334 ROS_DEBUG (
"Arm power is %s\tArm is %sconnected\n", (
armPowerOn ?
"on" :
"off"), (
armConnected ?
"" :
"not "));
335 ROS_DEBUG (
"Arm joint status:\n");
336 for (
int ii = 0; ii < 6; ii++)
344 ROS_DEBUG (
" |\tSpeed\tHome\tMin\tCentre\tMax\tTicks/90\n");
352 unsigned short newxpos, newypos;
355 cnt +=
sizeof(
unsigned char);
367 newxpos = ((buffer[cnt] | (buffer[cnt+1] << 8))
374 ROS_DEBUG(
"invalid odometry change [%d]; odometry values are tainted", change);
381 cnt +=
sizeof(short);
383 newypos = ((buffer[cnt] | (buffer[cnt+1] << 8))
390 ROS_DEBUG(
"invalid odometry change [%d]; odometry values are tainted", change);
397 cnt +=
sizeof(short);
400 rint(((
short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
402 cnt +=
sizeof(short);
405 rint(((
short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
407 cnt +=
sizeof(short);
410 rint(((
short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
412 cnt +=
sizeof(short);
415 cnt +=
sizeof(
unsigned char);
416 ROS_DEBUG(
"battery value: %d",
battery );
420 cnt +=
sizeof(
unsigned char);
424 cnt +=
sizeof(
unsigned char);
427 rint(((
short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
429 cnt +=
sizeof(short);
431 ptu = (buffer[cnt] | (buffer[cnt+1] << 8));
434 cnt +=
sizeof(short);
437 if(buffer[cnt] != 255 && buffer[cnt] != 0 && buffer[cnt] != 181)
439 cnt +=
sizeof(
unsigned char);
441 unsigned char numSonars=buffer[cnt];
442 cnt+=
sizeof(
unsigned char);
448 for(
unsigned char i=0;i<numSonars;i++)
450 unsigned char sonarIndex=buffer[cnt+i*(
sizeof(
unsigned char)+
sizeof(
unsigned short))];
451 if((sonarIndex+1)>maxSonars) maxSonars=sonarIndex+1;
457 unsigned short *newSonars=
new unsigned short[maxSonars];
462 sonarreadings=maxSonars;
466 for(
unsigned char i=0;i<numSonars;i++)
468 sonars[buffer[cnt]]= (
unsigned short)
469 rint((buffer[cnt+1] | (buffer[cnt+2] << 8)) *
471 cnt+=
sizeof(
unsigned char)+
sizeof(
unsigned short);
475 timer = (buffer[cnt] | (buffer[cnt+1] << 8));
476 cnt +=
sizeof(short);
479 cnt +=
sizeof(
unsigned char);
482 cnt +=
sizeof(
unsigned char);
485 cnt +=
sizeof(
unsigned char);
502 unsigned char type = buffer[1];
506 ROS_ERROR(
"Attempt to parse non SERAUX packet as serial data.\n");
510 int len = (int)buffer[0]-3;
520 for (ix = (len>18 ? len-17 : 2); ix<=len-8; ix++)
521 if (buffer[ix] == 255)
523 if (len < 10 || ix > len-8) {
524 ROS_ERROR(
"Failed to get a full blob tracking packet.\n");
529 if (buffer[ix+1] ==
'S')
532 ROS_DEBUG(
"Tracking color (RGB): %d %d %d\n"
533 " with variance: %d %d %d\n",
534 buffer[ix+2], buffer[ix+3], buffer[ix+4],
535 buffer[ix+5], buffer[ix+6], buffer[ix+7]);
536 blobcolor = buffer[ix+2]<<16 | buffer[ix+3]<<8 | buffer[ix+4];
541 if (buffer[ix+1] ==
'M')
556 ROS_ERROR(
"Unknown blob tracker packet type: %c\n", buffer[ix+1]);
571 int len = (int)buffer[0]-3;
573 unsigned char type = buffer[1];
577 ROS_ERROR(
"Attempt to parse non GYRO packet as gyro data.\n");
583 ROS_DEBUG(
"Couldn't get gyro measurement count");
588 int count = (int)buffer[2];
591 if((len-1) != (count*3))
593 ROS_DEBUG(
"Mismatch between gyro measurement count and packet length");
607 for(
int i=0; i<count; i++)
609 rate = (
unsigned short)(buffer[bufferpos++]);
610 rate |= buffer[bufferpos++] << 8;
616 int32_t average_rate = (int32_t)rint(ratesum / (
float)count);
624 int length = (int) buffer[0] - 2;
628 ROS_ERROR(
"Attempt to parse a non ARM packet as arm data.\n");
632 if (length < 1 || length != 9)
634 ROS_DEBUG (
"ARMpac length incorrect size");
638 unsigned char status = buffer[2];
649 unsigned char motionStatus = buffer[3];
650 if (motionStatus & 0x01)
652 if (motionStatus & 0x02)
654 if (motionStatus & 0x04)
656 if (motionStatus & 0x08)
658 if (motionStatus & 0x10)
660 if (motionStatus & 0x20)
669 int length = (int) buffer[0] - 2;
672 ROS_ERROR (
"Attempt to parse a non ARMINFO packet as arm info.\n");
678 ROS_DEBUG (
"ARMINFOpac length bad size");
unsigned char armJointPos[6]
p2os_msgs::SonarArray sonar
Container for sonar data.
void ParseGyro(unsigned char *buffer)
void ParseArmInfo(unsigned char *buffer)
p2os_msgs::AIO aio
Analog In/Out.
unsigned char motors_enabled
p2os_msgs::GripperState gripper
Provides the state of the gripper.
geometry_msgs::TransformStamped odom_trans
Transformed odometry frame.
void ParseSERAUX(unsigned char *buffer)
RobotParams_t PlayerRobotParams[]
void ParseStandard(unsigned char *buffer)
unsigned char sonarreadings
int PositionChange(unsigned short, unsigned short)
unsigned short frontbumpers
p2os_msgs::BatteryState batt
Provides the battery voltage.
void FillStandard(ros_p2os_data_t *data)
unsigned char armNumJoints
void ParseArm(unsigned char *buffer)
p2os_msgs::MotorState motors
Provides the state of the motors (enabled or disabled)
nav_msgs::Odometry position
Provides the position of the robot.
unsigned short rearbumpers
p2os_msgs::DIO dio
Digital In/Out.
double armJointPosRads[6]