Revision: 9066
http://playerstage.svn.sourceforge.net/playerstage/?rev=9066&view=rev
Author: jpgr87
Date: 2011-07-23 00:00:51 +0000 (Sat, 23 Jul 2011)
Log Message:
-----------
Added capability requests to roomba and create drivers.
Added position2d odometry set/reset to roomba and create drivers.
Moved search for canlib to searchforstuff.cmake
Separated CAN functionality in segwayrmp driver, driver can now build usb-only
Added capability requests to the segwayrmp driver
Modified Paths:
--------------
code/player/trunk/cmake/internal/SearchForStuff.cmake
code/player/trunk/config.h.in
code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc
code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc
code/player/trunk/server/drivers/mixed/rmp/CMakeLists.txt
code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h
code/player/trunk/server/drivers/mixed/rmp/rmp_frame.h
code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc
code/player/trunk/server/drivers/mixed/rmp/segwayrmp.h
Modified: code/player/trunk/cmake/internal/SearchForStuff.cmake
===================================================================
--- code/player/trunk/cmake/internal/SearchForStuff.cmake 2011-07-17
00:13:57 UTC (rev 9065)
+++ code/player/trunk/cmake/internal/SearchForStuff.cmake 2011-07-23
00:00:51 UTC (rev 9066)
@@ -283,3 +283,19 @@
SET(HAVE_PHIDGET true)
ENDIF (HAVE_PHIDGET_H)
+
+MARK_AS_ADVANCED (CANLIB_DIR)
+IF ("${CANLIB_DIR}" STREQUAL "")
+ SET (CANLIB_HEADER "canlib.h")
+ SET (CANLIB_INCLUDE_FLAGS "")
+ SET (CANLIB_LINK_FLAGS "-lcanlib")
+ELSE ("${CANLIB_DIR}" STREQUAL "")
+ SET (CANLIB_HEADER "${CANLIB_DIR}/include/canlib.h")
+ SET (CANLIB_INCLUDE_FLAGS "-I${CANLIB_DIR}/include")
+ SET (CANLIB_LINK_FLAGS "-L${CANLIB_DIR}/lib -lcanlib")
+ENDIF ("${CANLIB_DIR}" STREQUAL "")
+
+INCLUDE (CheckIncludeFile)
+SET(CMAKE_REQUIRED_DEFINITIONS ${CANLIB_INCLUDE_FLAGS})
+CHECK_INCLUDE_FILE("canlib.h" HAVE_CANLIB)
+
Modified: code/player/trunk/config.h.in
===================================================================
--- code/player/trunk/config.h.in 2011-07-17 00:13:57 UTC (rev 9065)
+++ code/player/trunk/config.h.in 2011-07-23 00:00:51 UTC (rev 9066)
@@ -17,4 +17,5 @@
#cmakedefine WORDS_BIGENDIAN 1
#cmakedefine HAVE_SETDLLDIRECTORY 1
#cmakedefine HAVE_PHIDGET_2_1_7 1
+#cmakedefine HAVE_CANLIB 1
Modified: code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc
2011-07-17 00:13:57 UTC (rev 9065)
+++ code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc
2011-07-23 00:00:51 UTC (rev 9066)
@@ -68,7 +68,11 @@
@par Supported configuration requests
- PLAYER_POSITION2D_REQ_GET_GEOM
+- PLAYER_POSITION2D_REQ_SET_ODOM
+- PLAYER_POSITION2D_REQ_RESET_ODOM
- PLAYER_BUMPER_REQ_GET_GEOM
+- PLAYER_IR_REQ_POSE
+- PLAYER_OPAQUE_REQ
@par Configuration file options
@@ -136,6 +140,9 @@
// full control or not
bool safe;
+
+ // offsets for position2d odom
+ double x_offset, y_offset, a_offset;
player_devaddr_t position_addr;
player_devaddr_t power_addr;
@@ -244,6 +251,9 @@
this->serial_port = cf->ReadString(section, "port", "/dev/ttyS0");
this->safe = cf->ReadInt(section, "safe", 1);
this->create_dev = NULL;
+ this->x_offset = 0;
+ this->y_offset = 0;
+ this->a_offset = 0;
}
Create::~Create()
@@ -297,9 +307,9 @@
player_position2d_data_t posdata;
memset(&posdata,0,sizeof(posdata));
- posdata.pos.px = this->create_dev->ox;
- posdata.pos.py = this->create_dev->oy;
- posdata.pos.pa = this->create_dev->oa;
+ posdata.pos.px = this->create_dev->ox - this->x_offset;
+ posdata.pos.py = this->create_dev->oy - this->y_offset;
+ posdata.pos.pa = this->create_dev->oa - this->a_offset;
this->Publish(this->position_addr,
PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
@@ -408,6 +418,24 @@
player_msghdr * hdr,
void * data)
{
+ // Let clients know which commands and requests this driver implements
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST (bumper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST (ir_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST (opaque_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST (gripper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL);
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_REQ_MOTOR_POWER);
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_REQ_SET_ODOM);
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_REQ_RESET_ODOM);
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_REQ_GET_GEOM);
+ HANDLE_CAPABILITY_REQUEST (bumper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_BUMPER_REQ_GET_GEOM);
+ HANDLE_CAPABILITY_REQUEST (ir_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_IR_REQ_POSE);
+ HANDLE_CAPABILITY_REQUEST (opaque_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_OPAQUE_REQ);
+ HANDLE_CAPABILITY_REQUEST (opaque_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD);
+ HANDLE_CAPABILITY_REQUEST (gripper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_GRIPPER_CMD_OPEN);
+ HANDLE_CAPABILITY_REQUEST (gripper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_GRIPPER_CMD_CLOSE);
if(Message::MatchMessage(hdr,
PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL,
@@ -436,6 +464,27 @@
return 0;
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
+ PLAYER_POSITION2D_REQ_SET_ODOM,
+ this->position_addr))
+ {
+ player_position2d_data_t *positionreq = (player_position2d_data_t*)data;
+
+ this->x_offset = this->create_dev->ox - positionreq->pos.px;
+ this->y_offset = this->create_dev->oy - positionreq->pos.py;
+ this->a_offset = this->create_dev->oa - positionreq->pos.pa;
+ return 0;
+
+ }
+ else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
+ PLAYER_POSITION2D_REQ_RESET_ODOM,
+ this->position_addr))
+ {
+ this->x_offset = this->create_dev->ox;
+ this->y_offset = this->create_dev->oy;
+ this->a_offset = this->create_dev->oa;
+ return 0;
+ }
+ else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_GET_GEOM,
this->position_addr))
{
Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc
2011-07-17 00:13:57 UTC (rev 9065)
+++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc
2011-07-23 00:00:51 UTC (rev 9066)
@@ -66,7 +66,11 @@
@par Supported configuration requests
- PLAYER_POSITION2D_REQ_GET_GEOM
+- PLAYER_POSITION2D_REQ_SET_ODOM
+- PLAYER_POSITION2D_REQ_RESET_ODOM
- PLAYER_BUMPER_REQ_GET_GEOM
+- PLAYER_IR_REQ_POSE
+- PLAYER_OPAQUE_REQ
@par Configuration file options
@@ -150,6 +154,9 @@
bool bumplocked;
bool roomba500;
+
+ // Offsets for setting & resetting position2d odometry
+ double x_offset, y_offset, a_offset;
player_devaddr_t position_addr;
player_devaddr_t power_addr;
@@ -262,6 +269,9 @@
this->bumplock = cf->ReadInt(section, "bumplock", 0);
this->roomba500 = cf->ReadBool(section, "roomba500", 0);
this->bumplocked = false;
+ this->x_offset = 0;
+ this->y_offset = 0;
+ this->a_offset = 0;
this->roomba_dev = NULL;
}
@@ -329,9 +339,9 @@
player_position2d_data_t posdata;
memset(&posdata,0,sizeof(posdata));
- posdata.pos.px = this->roomba_dev->ox;
- posdata.pos.py = this->roomba_dev->oy;
- posdata.pos.pa = this->roomba_dev->oa;
+ posdata.pos.px = this->roomba_dev->ox - this->x_offset;
+ posdata.pos.py = this->roomba_dev->oy - this->y_offset;
+ posdata.pos.pa = this->roomba_dev->oa - this->a_offset;
posdata.stall = static_cast<uint8_t>(this->bumplocked);
this->Publish(this->position_addr,
@@ -462,6 +472,25 @@
player_msghdr * hdr,
void * data)
{
+ // Let clients know which commands and requests this driver implements
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST (bumper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST (ir_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST (opaque_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST (gripper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL);
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_REQ_MOTOR_POWER);
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_REQ_SET_ODOM);
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_REQ_RESET_ODOM);
+ HANDLE_CAPABILITY_REQUEST (position_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_REQ_GET_GEOM);
+ HANDLE_CAPABILITY_REQUEST (bumper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_BUMPER_REQ_GET_GEOM);
+ HANDLE_CAPABILITY_REQUEST (ir_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_IR_REQ_POSE);
+ HANDLE_CAPABILITY_REQUEST (opaque_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_OPAQUE_REQ);
+ HANDLE_CAPABILITY_REQUEST (opaque_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD);
+ HANDLE_CAPABILITY_REQUEST (gripper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_GRIPPER_CMD_OPEN);
+ HANDLE_CAPABILITY_REQUEST (gripper_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_GRIPPER_CMD_CLOSE);
+
if(Message::MatchMessage(hdr,
PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL,
@@ -499,6 +528,27 @@
return 0;
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
+ PLAYER_POSITION2D_REQ_SET_ODOM,
+ this->position_addr))
+ {
+ player_position2d_data_t *positionreq = (player_position2d_data_t*)data;
+
+ this->x_offset = this->roomba_dev->ox - positionreq->pos.px;
+ this->y_offset = this->roomba_dev->oy - positionreq->pos.py;
+ this->a_offset = this->roomba_dev->oa - positionreq->pos.pa;
+ return 0;
+
+ }
+ else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
+ PLAYER_POSITION2D_REQ_RESET_ODOM,
+ this->position_addr))
+ {
+ this->x_offset = this->roomba_dev->ox;
+ this->y_offset = this->roomba_dev->oy;
+ this->a_offset = this->roomba_dev->oa;
+ return 0;
+ }
+ else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_GET_GEOM,
this->position_addr))
{
Modified: code/player/trunk/server/drivers/mixed/rmp/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/mixed/rmp/CMakeLists.txt 2011-07-17
00:13:57 UTC (rev 9065)
+++ code/player/trunk/server/drivers/mixed/rmp/CMakeLists.txt 2011-07-23
00:00:51 UTC (rev 9066)
@@ -1,16 +1,11 @@
-PLAYERDRIVER_OPTION (segwayrmp build_segwayrmp OFF "Disabled by default")
+PLAYERDRIVER_OPTION (segwayrmp build_segwayrmp ON "Build Segway RMP driver")
SET (CANLIB_DIR "" CACHE STRING "Directory containing the CANLIB headers and
libraries")
-MARK_AS_ADVANCED (CANLIB_DIR)
-IF ("${CANLIB_DIR}" STREQUAL "")
- SET (segwayrmpReqHeader "canlib.h")
- SET (segwayrmpExtraFlags "")
- SET (segwayrmpExtraLibs "-lcanlib")
-ELSE ("${CANLIB_DIR}" STREQUAL "")
- SET (segwayrmpReqHeader "${CANLIB_DIR}/include/canlib.h")
- SET (segwayrmpExtraFlags "-I${CANLIB_DIR}/include")
- SET (segwayrmpExtraLibs "-L${CANLIB_DIR}/lib -lcanlib")
-ENDIF ("${CANLIB_DIR}" STREQUAL "")
-PLAYERDRIVER_REQUIRE_HEADER (segwayrmp build_segwayrmp ${segwayrmpReqHeader})
+
+IF (${HAVE_CANLIB})
PLAYERDRIVER_ADD_DRIVER (segwayrmp build_segwayrmp
- LINKFLAGS ${segwayrmpExtraLibs} CFLAGS ${segwayrmpExtraFlags}
+ LINKFLAGS ${CANLIB_LINK_FLAGS} CFLAGS ${CANLIB_INCLUDE_FLAGS}
SOURCES segwayrmp.cc canio_kvaser.cc usb_packet.cc)
+ELSE (${HAVE_CANLIB})
+PLAYERDRIVER_ADD_DRIVER (segwayrmp build_segwayrmp
+ SOURCES segwayrmp.cc usb_packet.cc)
+ENDIF (${HAVE_CANLIB})
Modified: code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h
===================================================================
--- code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h 2011-07-17
00:13:57 UTC (rev 9065)
+++ code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h 2011-07-23
00:00:51 UTC (rev 9066)
@@ -37,5 +37,4 @@
virtual int WritePacket(CanPacket &pkt);
virtual int Shutdown();
};
-
#endif
Modified: code/player/trunk/server/drivers/mixed/rmp/rmp_frame.h
===================================================================
--- code/player/trunk/server/drivers/mixed/rmp/rmp_frame.h 2011-07-17
00:13:57 UTC (rev 9065)
+++ code/player/trunk/server/drivers/mixed/rmp/rmp_frame.h 2011-07-23
00:00:51 UTC (rev 9066)
@@ -18,11 +18,8 @@
*
*/
-
#include "canio.h"
-#include "canio_kvaser.h"
-
#define RMP_CAN_ID_SHUTDOWN 0x0412
#define RMP_CAN_ID_COMMAND 0x0413
#define RMP_CAN_ID_MSG1 0x0400
Modified: code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc 2011-07-17
00:13:57 UTC (rev 9065)
+++ code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc 2011-07-23
00:00:51 UTC (rev 9066)
@@ -251,6 +251,11 @@
{
bus_type = CANBUS;
PLAYER_MSG0(2, "Got CAN Bus");
+#if !defined HAVE_CANLIB
+ PLAYER_ERROR("CAN bus support not compiled into RMP driver.");
+ PLAYER_ERROR("Please rebuild driver with canlib.h");
+ this->SetError(-1);
+#endif
}
else if(strcmp( bus_setting, "usb" ) == 0 )
@@ -335,6 +340,7 @@
int SegwayRMP::CanBusSetup() {
+#if defined HAVE_CANLIB
PLAYER_MSG0(2, "CAN bus initializing");
if(!strcmp(this->caniotype, "kvaser"))
@@ -352,6 +358,11 @@
return(-1);
}
return 0;
+#else
+ PLAYER_ERROR("Error on CAN Init: CAN support not compiled into RMP driver");
+ PLAYER_ERROR("Please verify canlib.h is present & rebuild RMP driver");
+ return -1;
+#endif
}
int SegwayRMP::USBSetup()
@@ -581,7 +592,16 @@
{
/// @todo
/// Handle config requests
+ HANDLE_CAPABILITY_REQUEST(position_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST(position3d_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST(position_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL);
+ HANDLE_CAPABILITY_REQUEST(position_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER);
+ HANDLE_CAPABILITY_REQUEST(position_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_RESET_ODOM);
+
+ HANDLE_CAPABILITY_REQUEST(position3d_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_POSITION3D_CMD_SET_VEL);
+ HANDLE_CAPABILITY_REQUEST(position3d_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_POSITION3D_REQ_MOTOR_POWER);
+
// 2-D velocity command
if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL,
Modified: code/player/trunk/server/drivers/mixed/rmp/segwayrmp.h
===================================================================
--- code/player/trunk/server/drivers/mixed/rmp/segwayrmp.h 2011-07-17
00:13:57 UTC (rev 9065)
+++ code/player/trunk/server/drivers/mixed/rmp/segwayrmp.h 2011-07-23
00:00:51 UTC (rev 9066)
@@ -19,7 +19,10 @@
*/
#include "canio.h"
+#if defined HAVE_CANLIB
#include "canio_kvaser.h"
+#endif
+
#include "usb_packet.h"
#include <libplayercore/playercore.h>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Storage Efficiency Calculator
This modeling tool is based on patent-pending intellectual property that
has been used successfully in hundreds of IBM storage optimization engage-
ments, worldwide. Store less, Store more with what you own, Move data to
the right place. Try It Now! http://www.accelacomm.com/jaw/sfnl/114/51427378/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
