Update of /cvsroot/playerstage/code/player/server/drivers/mixed/rmp
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4552/server/drivers/mixed/rmp
Modified Files:
Tag: release-2-0-patches
Makefile.am segwayrmp.cc segwayrmp.h
Log Message:
backported lots of stuff from HEAD
Index: segwayrmp.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/rmp/segwayrmp.cc,v
retrieving revision 1.40
retrieving revision 1.40.2.1
diff -C2 -d -r1.40 -r1.40.2.1
*** segwayrmp.cc 23 Feb 2006 18:54:55 -0000 1.40
--- segwayrmp.cc 7 Jun 2006 16:12:51 -0000 1.40.2.1
***************
*** 34,37 ****
--- 34,40 ----
the Player 2.0 API.
+ The driver has been updated to the point that it compiles, but a few key
requests
+ have been disabled and it has not been tested in anyway.
+
The segwayrmp driver provides control of a Segway RMP (Robotic
Mobility Platform), which is an experimental robotic version of the
***************
*** 338,349 ****
// Send data to clients
! PutData(this->position_id, &this->position_data,
! sizeof(this->position_data), NULL);
! PutData(this->position3d_id, &this->position3d_data,
! sizeof(this->position3d_data), NULL);
! PutData(this->power_id, &this->power_data,
! sizeof(this->power_data), NULL);
! // check for config requests from the position interface
if((buffer_len = GetConfig(this->position_id, &client, buffer,
sizeof(buffer),NULL)) > 0)
{
--- 341,352 ----
// Send data to clients
! Publish(this->position_id, NULL, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
! &this->position_data, sizeof(this->position_data), NULL);
! Publish(this->position3d_id, NULL, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION3D_DATA_STATE,
! &this->position3d_data, sizeof(this->position3d_data), NULL);
! Publish(this->power_id, NULL, PLAYER_MSGTYPE_DATA,
PLAYER_POWER_DATA_STATE,
! &this->power_data, sizeof(this->power_data), NULL);
! /* // check for config requests from the position interface
if((buffer_len = GetConfig(this->position_id, &client, buffer,
sizeof(buffer),NULL)) > 0)
{
***************
*** 410,423 ****
}
// No commands, so we may timeout soon
! if (!got_command)
timeout_counter++;
if(timeout_counter >= RMP_TIMEOUT_CYCLES)
{
! if(xspeed || yawspeed)
{
PLAYER_WARN("timeout exceeded without new commands; stopping robot");
! xspeed = 0;
! yawspeed = 0;
}
// set it to the limit, to prevent overflow, but keep the robot
--- 413,426 ----
}
// No commands, so we may timeout soon
! if (!got_command)*/
timeout_counter++;
if(timeout_counter >= RMP_TIMEOUT_CYCLES)
{
! if(curr_xspeed || curr_yawspeed)
{
PLAYER_WARN("timeout exceeded without new commands; stopping robot");
! curr_xspeed = 0;
! curr_yawspeed = 0;
}
// set it to the limit, to prevent overflow, but keep the robot
***************
*** 428,437 ****
if(!motor_enabled)
{
! xspeed = 0;
! yawspeed = 0;
}
// make a velocity command... could be zero
! MakeVelocityCommand(&pkt,xspeed,yawspeed);
if(Write(pkt) < 0)
PLAYER_ERROR("error on write");
--- 431,440 ----
if(!motor_enabled)
{
! curr_xspeed = 0;
! curr_yawspeed = 0;
}
// make a velocity command... could be zero
! MakeVelocityCommand(&pkt,static_cast<int> (curr_xspeed),static_cast<int>
(curr_yawspeed));
if(Write(pkt) < 0)
PLAYER_ERROR("error on write");
***************
*** 457,460 ****
--- 460,464 ----
this->motor_enabled = cmd->state & this->motor_allow_enable;
this->timeout_counter = 0;
+ return 0;
}
// 3-D velocity command
***************
*** 465,474 ****
player_position3d_cmd_vel_t* cmd = (player_position3d_cmd_vel_t*)data;
this->curr_xspeed = cmd->vel.px;
! this->curr_yawspeed = cmd->vel.pa;
this->motor_enabled = cmd->state & this->motor_allow_enable;
this->timeout_counter = 0;
}
! else
! return(-1);
}
--- 469,493 ----
player_position3d_cmd_vel_t* cmd = (player_position3d_cmd_vel_t*)data;
this->curr_xspeed = cmd->vel.px;
! this->curr_yawspeed = cmd->vel.pyaw;
this->motor_enabled = cmd->state & this->motor_allow_enable;
this->timeout_counter = 0;
+ return 0;
}
!
! if (hdr->type == PLAYER_MSGTYPE_REQ && hdr->addr.interf ==
position_id.interf
! && hdr->addr.index == position_id.index)
! {
! return HandlePositionConfig(resp_queue, hdr->subtype, data, hdr->size);
! }
!
! if (hdr->type == PLAYER_MSGTYPE_REQ && hdr->addr.interf ==
position3d_id.interf
! && hdr->addr.index == position3d_id.index)
! {
! return HandlePosition3DConfig(resp_queue, hdr->subtype, data, hdr->size);
! }
!
!
!
! return(-1);
}
***************
*** 477,494 ****
// returns 0 to indicate we did NOT write to CAN bus
int
! SegwayRMP::HandlePositionConfig(void* client, unsigned char* buffer, size_t
len)
{
uint16_t rmp_cmd,rmp_val;
! player_rmp_config_t *rmp;
CanPacket pkt;
! switch(buffer[0])
{
! case PLAYER_POSITION_MOTOR_POWER_REQ:
// just set a flag telling us whether we should
// act on motor commands
// set the commands to 0... think it will automatically
// do this for us.
! if(buffer[1])
this->motor_allow_enable = true;
else
--- 496,513 ----
// returns 0 to indicate we did NOT write to CAN bus
int
! SegwayRMP::HandlePositionConfig(MessageQueue* client, unsigned int subtype,
void* buffer, size_t len)
{
uint16_t rmp_cmd,rmp_val;
! //player_rmp_config_t *rmp;
CanPacket pkt;
! switch(subtype)
{
! case PLAYER_POSITION2D_REQ_MOTOR_POWER:
// just set a flag telling us whether we should
// act on motor commands
// set the commands to 0... think it will automatically
// do this for us.
! if(((char *) buffer)[0])
this->motor_allow_enable = true;
else
***************
*** 497,518 ****
printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable);
! if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL))
! PLAYER_ERROR("Failed to PutReply in segwayrmp\n");
! break;
! case PLAYER_POSITION_GET_GEOM_REQ:
! player_position_geom_t geom;
! geom.subtype = PLAYER_POSITION_GET_GEOM_REQ;
! geom.pose[0] = htons((short)(0));
! geom.pose[1] = htons((short)(0));
! geom.pose[2] = htons((short)(0));
! geom.size[0] = htons((short)(508));
! geom.size[1] = htons((short)(610));
! if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK, &geom, sizeof(geom),NULL))
! PLAYER_ERROR("Segway: failed to PutReply");
! break;
! case PLAYER_POSITION_RESET_ODOM_REQ:
// we'll reset all the integrators
--- 516,534 ----
printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable);
! Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK,subtype);
! return 0;
! case PLAYER_POSITION2D_REQ_GET_GEOM:
! player_position2d_geom_t geom;
! geom.pose.px = 0;
! geom.pose.py = 0;
! geom.pose.pa = 0;
! geom.size.sw = 0.508;
! geom.size.sl = 0.610;
! Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_GET_GEOM, &geom, sizeof(geom),NULL);
! return 0;
! case PLAYER_POSITION2D_REQ_RESET_ODOM:
// we'll reset all the integrators
***************
*** 521,526 ****
if(Write(pkt) < 0)
{
! if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
! PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
}
else
--- 537,541 ----
if(Write(pkt) < 0)
{
! Publish(position_id, client,
PLAYER_MSGTYPE_RESP_NACK,PLAYER_POSITION2D_REQ_RESET_ODOM);
}
else
***************
*** 528,536 ****
if (Write(pkt) < 0) {
! if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
! PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
} else {
! if(PutReply(client, PLAYER_MSGTYPE_RESP_ACK,NULL))
! PLAYER_ERROR("SEGWAY: Failed to PutReply\n");
}
}
--- 543,549 ----
if (Write(pkt) < 0) {
! Publish(position_id, client,
PLAYER_MSGTYPE_RESP_NACK,PLAYER_POSITION2D_REQ_RESET_ODOM);
} else {
! Publish(position_id, client,
PLAYER_MSGTYPE_RESP_ACK,PLAYER_POSITION2D_REQ_RESET_ODOM);
}
}
***************
*** 540,546 ****
// return 1 to indicate that we wrote to the CAN bus this time
! return(1);
! case PLAYER_POSITION_RMP_VELOCITY_SCALE:
rmp_cmd = RMP_CAN_CMD_MAX_VEL;
rmp = (player_rmp_config_t *)buffer;
--- 553,559 ----
// return 1 to indicate that we wrote to the CAN bus this time
! return(0);
! /* case PLAYER_POSITION_RMP_VELOCITY_SCALE:
rmp_cmd = RMP_CAN_CMD_MAX_VEL;
rmp = (player_rmp_config_t *)buffer;
***************
*** 684,698 ****
// return 1 to indicate that we wrote to the CAN bus this time
return(1);
!
default:
printf("segwayrmp received unknown config request %d\n",
! buffer[0]);
! if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
! PLAYER_ERROR("Failed to PutReply in segwayrmp\n");
break;
}
! // return 0, to indicate that we did NOT write to the CAN bus this time
! return(0);
}
--- 697,711 ----
// return 1 to indicate that we wrote to the CAN bus this time
return(1);
! */
default:
printf("segwayrmp received unknown config request %d\n",
! subtype);
! /* if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL))
! PLAYER_ERROR("Failed to PutReply in segwayrmp\n");*/
break;
}
! // return -1, to indicate that we did NOT handle the message
! return(-1);
}
***************
*** 701,714 ****
// returns 0 to indicate we did NOT write to CAN bus
int
! SegwayRMP::HandlePosition3DConfig(void* client, unsigned char* buffer, size_t
len)
{
! switch(buffer[0])
{
! case PLAYER_POSITION3D_MOTOR_POWER_REQ:
// just set a flag telling us whether we should
// act on motor commands
// set the commands to 0... think it will automatically
// do this for us.
! if(buffer[1])
this->motor_allow_enable = true;
else
--- 714,727 ----
// returns 0 to indicate we did NOT write to CAN bus
int
! SegwayRMP::HandlePosition3DConfig(MessageQueue* client, uint32_t subtype,
void* buffer, size_t len)
{
! switch(subtype)
{
! case PLAYER_POSITION3D_MOTOR_POWER:
// just set a flag telling us whether we should
// act on motor commands
// set the commands to 0... think it will automatically
// do this for us.
! if(((char*)buffer)[0])
this->motor_allow_enable = true;
else
***************
*** 717,734 ****
printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable);
! if(PutReply(this->position3d_id, client, PLAYER_MSGTYPE_RESP_ACK, NULL))
! PLAYER_ERROR("Failed to PutReply in segwayrmp\n");
! break;
default:
printf("segwayrmp received unknown config request %d\n",
! buffer[0]);
! if(PutReply(this->position3d_id, client, PLAYER_MSGTYPE_RESP_NACK,NULL))
! PLAYER_ERROR("Failed to PutReply in segwayrmp\n");
! break;
}
! // return 0, to indicate that we did NOT write to the CAN bus this time
! return(0);
}
--- 730,743 ----
printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable);
! Publish(this->position3d_id, client, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION3D_MOTOR_POWER);
! return 0;
default:
printf("segwayrmp received unknown config request %d\n",
! subtype);
}
! // return -1, to indicate that we did process the message
! return(-1);
}
***************
*** 825,829 ****
this->position_data.pos.px = this->odom_x;
this->position_data.pos.py = this->odom_y;
! this->position_data.pos.pa = this->odom_a;
// combine left and right wheel velocity to get foreward velocity
--- 834,838 ----
this->position_data.pos.px = this->odom_x;
this->position_data.pos.py = this->odom_y;
! this->position_data.pos.pa = this->odom_yaw;
// combine left and right wheel velocity to get foreward velocity
***************
*** 858,862 ****
if(tmp < 0)
tmp += 2*M_PI;
! this->position3d_data.roll = htonl((int32_t)rint(tmp * 1000.0));
// normalize angles to [0,360]
--- 867,871 ----
if(tmp < 0)
tmp += 2*M_PI;
! this->position3d_data.pos.proll = tmp;//htonl((int32_t)rint(tmp * 1000.0));
// normalize angles to [0,360]
***************
*** 865,889 ****
if(tmp < 0)
tmp += 2*M_PI;
! this->position3d_data.pitch = htonl((int32_t)rint(tmp * 1000.0));
! this->position3d_data.yaw = htonl(((int32_t)(this->odom_yaw * 1000.0)));
// combine left and right wheel velocity to get foreward velocity
! // change from counts/s into mm/s
! this->position3d_data.xspeed =
! htonl((uint32_t)rint(((double)data_frame->left_dot +
(double)data_frame->right_dot) /
(double)RMP_COUNT_PER_M_PER_S
! * 1000.0 / 2.0));
// no side or vertical speeds for this bot
! this->position3d_data.yspeed = 0;
! this->position3d_data.zspeed = 0;
! this->position3d_data.rollspeed =
! htonl((int32_t)rint((double)data_frame->roll_dot /
! (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180 *
1000.0));
! this->position3d_data.pitchspeed =
! htonl((int32_t)rint((double)data_frame->pitch_dot /
! (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180 *
1000.00));
// from counts/sec into millirad/sec. also, take the additive
// inverse, since the RMP reports clockwise angular velocity as
--- 874,898 ----
if(tmp < 0)
tmp += 2*M_PI;
! this->position3d_data.pos.ppitch = tmp;//htonl((int32_t)rint(tmp * 1000.0));
! this->position3d_data.pos.pyaw = tmp;//htonl(((int32_t)(this->odom_yaw *
1000.0)));
// combine left and right wheel velocity to get foreward velocity
! // change from counts/s into m/s
! this->position3d_data.vel.px =
! ((double)data_frame->left_dot +
(double)data_frame->right_dot) /
(double)RMP_COUNT_PER_M_PER_S
! / 2.0;
// no side or vertical speeds for this bot
! this->position3d_data.vel.py = 0;
! this->position3d_data.vel.pz = 0;
! this->position3d_data.vel.proll =
! (double)data_frame->roll_dot /
! (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180;
! this->position3d_data.vel.ppitch =
! (double)data_frame->pitch_dot /
! (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180;
// from counts/sec into millirad/sec. also, take the additive
// inverse, since the RMP reports clockwise angular velocity as
***************
*** 891,897 ****
// This one uses left_dot and right_dot, which comes from odometry
! this->position3d_data.yawspeed =
! htonl((int32_t)(rint((double)(data_frame->right_dot -
data_frame->left_dot) /
! (RMP_COUNT_PER_M_PER_S * RMP_GEOM_WHEEL_SEP * M_PI)
* 1000)));
// This one uses yaw_dot, which comes from the IMU
//data.position3d_data.yawspeed =
--- 900,906 ----
// This one uses left_dot and right_dot, which comes from odometry
! this->position3d_data.vel.pyaw =
! (double)(data_frame->right_dot - data_frame->left_dot) /
! (RMP_COUNT_PER_M_PER_S * RMP_GEOM_WHEEL_SEP * M_PI);
// This one uses yaw_dot, which comes from the IMU
//data.position3d_data.yawspeed =
***************
*** 904,909 ****
// and the specs for the HT say that it's a 72 volt system. assuming
// that the RMP is the same, we'll convert to decivolts for Player.
! this->power_data.charge =
! ntohs((uint16_t)rint(data_frame->battery * 7.2));
firstread = false;
--- 913,918 ----
// and the specs for the HT say that it's a 72 volt system. assuming
// that the RMP is the same, we'll convert to decivolts for Player.
! this->power_data.volts =
! data_frame->battery * 72;
firstread = false;
***************
*** 932,936 ****
pkt->PutByte(7, val);
! trans = (int16_t) rint((double)this->last_xspeed *
(double)RMP_COUNT_PER_MM_PER_S);
--- 941,945 ----
pkt->PutByte(7, val);
! trans = (int16_t) rint((double)this->curr_xspeed *
(double)RMP_COUNT_PER_MM_PER_S);
***************
*** 940,944 ****
trans = -RMP_MAX_TRANS_VEL_COUNT;
! rot = (int16_t) rint((double)this->last_yawspeed *
(double)RMP_COUNT_PER_DEG_PER_SS);
--- 949,953 ----
trans = -RMP_MAX_TRANS_VEL_COUNT;
! rot = (int16_t) rint((double)this->curr_yawspeed *
(double)RMP_COUNT_PER_DEG_PER_SS);
***************
*** 989,993 ****
}
! this->last_xspeed = xspeed;
int16_t trans = (int16_t) rint((double)xspeed *
--- 998,1002 ----
}
! this->curr_xspeed = xspeed;
int16_t trans = (int16_t) rint((double)xspeed *
***************
*** 1011,1015 ****
yawspeed = -this->max_yawspeed;
}
! this->last_yawspeed = yawspeed;
// rotational RMP command \in [-1024, 1024]
--- 1020,1024 ----
yawspeed = -this->max_yawspeed;
}
! this->curr_yawspeed = yawspeed;
// rotational RMP command \in [-1024, 1024]
Index: segwayrmp.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/rmp/segwayrmp.h,v
retrieving revision 1.5
retrieving revision 1.5.2.1
diff -C2 -d -r1.5 -r1.5.2.1
*** segwayrmp.h 15 Feb 2006 06:48:38 -0000 1.5
--- segwayrmp.h 7 Jun 2006 16:12:51 -0000 1.5.2.1
***************
*** 86,93 ****
// helper to handle config requests
! int HandlePositionConfig(void* client, unsigned char* buffer, size_t len);
// helper to handle config requests
! int HandlePosition3DConfig(void* client, unsigned char* buffer, size_t
len);
// helper to read a cycle of data from the RMP
--- 86,93 ----
// helper to handle config requests
! int HandlePositionConfig(MessageQueue* resp_queue, uint32_t subtype,
void* data, size_t len);
// helper to handle config requests
! int HandlePosition3DConfig(MessageQueue* resp_queue, uint32_t subtype,
void* data, size_t len);
// helper to read a cycle of data from the RMP
Index: Makefile.am
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/rmp/Makefile.am,v
retrieving revision 1.8
retrieving revision 1.8.10.1
diff -C2 -d -r1.8 -r1.8.10.1
*** Makefile.am 7 Sep 2004 00:20:45 -0000 1.8
--- Makefile.am 7 Jun 2006 16:12:51 -0000 1.8.10.1
***************
*** 1,10 ****
! noinst_LIBRARIES = @SEGWAYRMP_LIB@
!
! AM_CPPFLAGS = -Wall @SEGWAYRMP_EXTRA_CPPFLAGS@ -I$(top_srcdir)/server
!
! EXTRA_LIBRARIES = libsegwayrmp.a
- libsegwayrmp_a_SOURCES = segwayrmp.h segwayrmp.cc rmp_frame.h \
-
canio.h canio_kvaser.h canio_kvaser.cc
--- 1,10 ----
+ noinst_LTLIBRARIES =
+ if INCLUDE_SEGWAYRMP
+ noinst_LTLIBRARIES += libsegwayrmp.la
+ endif
+ AM_CPPFLAGS = -Wall -I$(top_srcdir) @SEGWAYRMP_EXTRA_CPPFLAGS@
! libsegwayrmp_la_SOURCES = segwayrmp.h segwayrmp.cc rmp_frame.h \
! canio.h canio_kvaser.h canio_kvaser.cc
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
