Update of /cvsroot/playerstage/code/player/server/drivers/mixed/irobot/roomba
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv15294/server/drivers/mixed/irobot/roomba
Modified Files:
roomba_driver.cc
Log Message:
applied patch [ 1820854 ] big Roomba patch
Index: roomba_driver.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/irobot/roomba/roomba_driver.cc,v
retrieving revision 1.17
retrieving revision 1.18
diff -C2 -d -r1.17 -r1.18
*** roomba_driver.cc 1 Nov 2007 22:16:20 -0000 1.17
--- roomba_driver.cc 23 Nov 2007 10:08:24 -0000 1.18
***************
*** 78,81 ****
--- 78,84 ----
- Nonzero to keep the robot in "safe" mode (the robot will stop if
the wheeldrop or cliff sensors are triggered), zero for "full" mode
+ - bumplock (integer)
+ - Default: 0
+ - If set to 1, the robot will stop whenever bumpers are closed
@par Example
***************
*** 104,113 ****
#include <unistd.h>
!
#include <libplayercore/playercore.h>
#include "roomba_comms.h"
! #define CYCLE_TIME_US 200000
class Roomba : public Driver
--- 107,120 ----
#include <unistd.h>
! #include <stddef.h>
! #include <stdio.h>
! #include <string.h>
! #include <pthread.h>
! #include <time.h>
#include <libplayercore/playercore.h>
#include "roomba_comms.h"
! #define CYCLE_TIME_NS 200000000
class Roomba : public Driver
***************
*** 134,137 ****
--- 141,147 ----
bool safe;
+ bool bumplock;
+ bool bumplocked;
+
player_devaddr_t position_addr;
player_devaddr_t power_addr;
***************
*** 143,146 ****
--- 153,162 ----
// The underlying roomba object
roomba_comm_t* roomba_dev;
+
+ player_position2d_cmd_vel_t position_cmd;
+ player_position2d_geom_t pos_geom;
+ player_bumper_geom_t bump_geom;
+ player_ir_pose poses;
+ player_opaque_data_t opaque_data;
};
***************
*** 165,169 ****
memset(&this->ir_addr,0,sizeof(player_devaddr_t));
memset(&this->opaque_addr,0,sizeof(player_devaddr_t));
! //memset(&this->gripper_addr,0,sizeof(player_devaddr_t));
// Do we create a position interface?
--- 181,185 ----
memset(&this->ir_addr,0,sizeof(player_devaddr_t));
memset(&this->opaque_addr,0,sizeof(player_devaddr_t));
! memset(&this->gripper_addr,0,sizeof(player_devaddr_t));
// Do we create a position interface?
***************
*** 236,239 ****
--- 252,257 ----
this->serial_port = cf->ReadString(section, "port", "/dev/ttyS0");
this->safe = cf->ReadInt(section, "safe", 1);
+ this->bumplock = cf->ReadInt(section, "bumplock", 0);
+ this->bumplocked = false;
this->roomba_dev = NULL;
}
***************
*** 252,256 ****
}
! /*printf("HERE\n");
roomba_set_leds(this->roomba_dev, 1, 0, 1, 0, 2, 128, 255);
*/
--- 270,274 ----
}
! /*
roomba_set_leds(this->roomba_dev, 1, 0, 1, 0, 2, 128, 255);
*/
***************
*** 278,286 ****
Roomba::Main()
{
! for(;;)
{
this->ProcessMessages();
! if(roomba_get_sensors(this->roomba_dev, -1) < 0)
{
PLAYER_ERROR("failed to get sensor data from roomba");
--- 296,306 ----
Roomba::Main()
{
! for (;;)
{
+ pthread_testcancel();
+
this->ProcessMessages();
! if (roomba_get_sensors(this->roomba_dev, -1) < 0)
{
PLAYER_ERROR("failed to get sensor data from roomba");
***************
*** 288,291 ****
--- 308,320 ----
return;
}
+ if ((this->bumplock) && ((this->roomba_dev->bumper_left) ||
(this->roomba_dev->bumper_right)))
+ {
+ this->bumplocked = true;
+ if (roomba_set_speeds(this->roomba_dev, 0.0, 0.0) < 0)
+ {
+ PLAYER_ERROR("failed to stop roomba");
+ }
+ }
+ else this->bumplocked = false;
////////////////////////////
***************
*** 397,401 ****
delete [] cpdata.data;
! usleep(CYCLE_TIME_US);
}
}
--- 426,433 ----
delete [] cpdata.data;
! struct timespec ts;
! ts.tv_sec = 0;
! ts.tv_nsec = CYCLE_TIME_NS;
! nanosleep(&ts, NULL);
}
}
***************
*** 412,417 ****
{
// get and send the latest motor command
! player_position2d_cmd_vel_t position_cmd;
! position_cmd = *(player_position2d_cmd_vel_t*)data;
PLAYER_MSG2(2,"sending motor commands %f:%f",
position_cmd.vel.px,
--- 444,458 ----
{
// get and send the latest motor command
! if (!data)
! {
! PLAYER_ERROR("NULL position command");
! return -1;
! }
! memcpy(&position_cmd, data, sizeof position_cmd);
! if (this->bumplocked)
! {
! if (position_cmd.vel.px > 0.0) position_cmd.vel.px = 0.0;
! position_cmd.vel.pa = 0.0;
! }
PLAYER_MSG2(2,"sending motor commands %f:%f",
position_cmd.vel.px,
***************
*** 438,452 ****
{
/* Return the robot geometry. */
! player_position2d_geom_t geom={{0}};
// Assume that it turns about its geometric center, so zeros are fine
! geom.size.sl = ROOMBA_DIAMETER;
! geom.size.sw = ROOMBA_DIAMETER;
this->Publish(this->position_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_GET_GEOM,
! (void*)&geom, sizeof(geom), NULL);
! return(0);
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
--- 479,493 ----
{
/* Return the robot geometry. */
! memset(&pos_geom, 0, sizeof pos_geom);
// Assume that it turns about its geometric center, so zeros are fine
! pos_geom.size.sl = ROOMBA_DIAMETER;
! pos_geom.size.sw = ROOMBA_DIAMETER;
this->Publish(this->position_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_GET_GEOM,
! (void*)&pos_geom, sizeof pos_geom, NULL);
! return 0;
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
***************
*** 454,481 ****
this->bumper_addr))
{
! player_bumper_geom_t geom;
! geom.bumper_def_count = 2;
! geom.bumper_def = new player_bumper_define_t[geom.bumper_def_count];
! geom.bumper_def[0].pose.px = 0.0;
! geom.bumper_def[0].pose.py = 0.0;
! geom.bumper_def[0].pose.pyaw = 0.0;
! geom.bumper_def[0].length = 0.0;
! geom.bumper_def[0].radius = ROOMBA_DIAMETER/2.0;
! geom.bumper_def[1].pose.px = 0.0;
! geom.bumper_def[1].pose.py = 0.0;
! geom.bumper_def[1].pose.pyaw = 0.0;
! geom.bumper_def[1].length = 0.0;
! geom.bumper_def[1].radius = ROOMBA_DIAMETER/2.0;
this->Publish(this->bumper_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_BUMPER_REQ_GET_GEOM,
! (void*)&geom);
! delete [] geom.bumper_def;
! return(0);
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
--- 495,528 ----
this->bumper_addr))
{
! memset(&bump_geom, 0, sizeof bump_geom);
! bump_geom.bumper_def_count = 2;
! bump_geom.bumper_def = new
player_bumper_define_t[bump_geom.bumper_def_count];
! if (!(bump_geom.bumper_def))
! {
! PLAYER_ERROR("Out of memory");
! return -1;
! }
! bump_geom.bumper_def[0].pose.px = 0.12;
! bump_geom.bumper_def[0].pose.py = 0.12;
! bump_geom.bumper_def[0].pose.pyaw = 45.0;
! bump_geom.bumper_def[0].length = 0.33;
! bump_geom.bumper_def[0].radius = ROOMBA_DIAMETER / 2.0;
! bump_geom.bumper_def[1].pose.px = 0.12;
! bump_geom.bumper_def[1].pose.py = -0.12;
! bump_geom.bumper_def[1].pose.pyaw = -45.0;
! bump_geom.bumper_def[1].length = 0.33;
! bump_geom.bumper_def[1].radius = ROOMBA_DIAMETER / 2.0;
this->Publish(this->bumper_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_BUMPER_REQ_GET_GEOM,
! (void*)&bump_geom);
! delete []bump_geom.bumper_def;
! bump_geom.bumper_def = NULL;
! return 0;
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
***************
*** 483,490 ****
this->ir_addr))
{
! player_ir_pose poses;
poses.poses_count = 11;
poses.poses = new player_pose3d_t[poses.poses_count];
// TODO: Fill in proper values
--- 530,542 ----
this->ir_addr))
{
! memset(&poses, 0, sizeof poses);
poses.poses_count = 11;
poses.poses = new player_pose3d_t[poses.poses_count];
+ if (!(poses.poses))
+ {
+ PLAYER_ERROR("Out of memory");
+ return -1;
+ }
// TODO: Fill in proper values
***************
*** 500,505 ****
PLAYER_IR_REQ_POSE,
(void*)&poses);
! delete [] poses.poses;
! return(0);
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,
--- 552,558 ----
PLAYER_IR_REQ_POSE,
(void*)&poses);
! delete []poses.poses;
! poses.poses = NULL;
! return 0;
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,
***************
*** 507,518 ****
this->opaque_addr))
{
! printf("Opaque\n");
! player_opaque_data_t opaque_data;
!
! printf("1\n");
! opaque_data = *(player_opaque_data_t*)data;
!
! printf("2\n");
! printf("Opaque Command\n");
// Play Command
--- 560,569 ----
this->opaque_addr))
{
! if (!data)
! {
! PLAYER_ERROR("NULL opaque data");
! return -1;
! }
! memcpy(&opaque_data, data, sizeof opaque_data);
// Play Command
***************
*** 583,587 ****
else
{
! return(-1);
}
}
--- 634,638 ----
else
{
! return -1;
}
}
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2005.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
