Update of /cvsroot/playerstage/code/player/server/drivers/position/isense
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4552/server/drivers/position/isense
Modified Files:
Tag: release-2-0-patches
Makefile.am inertiacube2.cc
Log Message:
backported lots of stuff from HEAD
Index: inertiacube2.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/isense/inertiacube2.cc,v
retrieving revision 1.7
retrieving revision 1.7.2.1
diff -C2 -d -r1.7 -r1.7.2.1
*** inertiacube2.cc 23 Feb 2006 18:54:55 -0000 1.7
--- inertiacube2.cc 7 Jun 2006 16:12:52 -0000 1.7.2.1
***************
*** 111,118 ****
#include <isense/isense.h>
! #include "player.h"
! #include "driver.h"
! #include "devicetable.h"
! #include "drivertable.h"
--- 111,115 ----
#include <isense/isense.h>
! #include <libplayercore/playercore.h>
***************
*** 144,159 ****
private: virtual void Main();
! // Process requests. Returns 1 if the configuration has changed.
! private: int HandleRequests();
!
! // Handle geometry requests.
! private: void HandleGetGeom(void *client, void *req, int reqlen);
// Update the InertiaCube.
private: void UpdateImu();
- // Update the position device (returns non-zero if changed).
- private: int UpdatePosition();
-
// Generate a new pose estimate.
private: void UpdatePose();
--- 141,150 ----
private: virtual void Main();
! // Process incoming messages from clients
! int ProcessMessage (MessageQueue * resp_queue, player_msghdr * hdr, void *
data);
// Update the InertiaCube.
private: void UpdateImu();
// Generate a new pose estimate.
private: void UpdatePose();
***************
*** 163,167 ****
// Geometry of underlying position device.
! private: player_position_geom_t geom;
// Compass setting (0 = off, 1 = partial, 2 = full).
--- 154,158 ----
// Geometry of underlying position device.
! private: player_position2d_geom_t geom;
// Compass setting (0 = off, 1 = partial, 2 = full).
***************
*** 172,180 ****
// Position device info (the one we are subscribed to).
! private: player_device_id_t position_id;
! private: Driver *position;
private: double position_time;
! private: double position_old_pose[3];
! private: double position_new_pose[3];
// Handle to the imu tracker.
--- 163,171 ----
// Position device info (the one we are subscribed to).
! private: player_devaddr_t position_id;
! private: Device *position;
private: double position_time;
! private: player_pose_t position_old_pose;
! private: player_pose_t position_new_pose;
// Handle to the imu tracker.
***************
*** 184,188 ****
// Combined pose estimate.
! private: double pose[3];
};
--- 175,179 ----
// Combined pose estimate.
! private: player_pose_t pose;
};
***************
*** 191,200 ****
Driver* InertiaCube2_Init( ConfigFile* cf, int section)
{
- if (strcmp( PLAYER_POSITION_STRING) != 0)
- {
- PLAYER_ERROR1("driver \"inertiacube2\" does not support interface
\"%s\"\n",
- interface);
- return (NULL);
- }
return ((Driver*) (new InertiaCube2( cf, section)));
}
--- 182,185 ----
***************
*** 204,208 ****
void InertiaCube2_Register(DriverTable* table)
{
! table->AddDriver("inertiacube2", PLAYER_READ_MODE, InertiaCube2_Init);
}
--- 189,193 ----
void InertiaCube2_Register(DriverTable* table)
{
! table->AddDriver("inertiacube2", InertiaCube2_Init);
}
***************
*** 211,221 ****
// Constructor
InertiaCube2::InertiaCube2( ConfigFile* cf, int section)
! : Driver(cf, section, sizeof(player_position_data_t), 0, 10, 10)
{
this->port = cf->ReadString(section, "port", "/dev/ttyS3");
// Must have a position device
! if (cf->ReadDeviceId(&this->position_id, section, "requires",
! PLAYER_POSITION_CODE, -1, NULL) != 0)
{
this->SetError(-1);
--- 196,206 ----
// Constructor
InertiaCube2::InertiaCube2( ConfigFile* cf, int section)
! : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_POSITION2D_CODE)
{
this->port = cf->ReadString(section, "port", "/dev/ttyS3");
// Must have a position device
! if (cf->ReadDeviceAddr(&this->position_id, section, "requires",
! PLAYER_POSITION2D_CODE, -1, NULL) != 0)
{
this->SetError(-1);
***************
*** 271,284 ****
int InertiaCube2::SetupPosition()
{
! // Subscribe to the position device.
! this->position = deviceTable->GetDriver(this->position_id);
! if (!this->position)
{
! PLAYER_ERROR("unable to locate suitable position device");
return -1;
}
! if (this->position->Subscribe(this) != 0)
{
! PLAYER_ERROR("unable to subscribe to position device");
return -1;
}
--- 256,273 ----
int InertiaCube2::SetupPosition()
{
! // Subscribe to the positino device.
! if (Device::MatchDeviceAddress (position_id, device_addr))
{
! PLAYER_ERROR ("attempt to subscribe to self");
return -1;
}
! if (!(position = deviceTable->GetDevice (position_id)))
{
! PLAYER_ERROR ("unable to locate suitable camera device");
! return -1;
! }
! if (position->Subscribe (InQueue) != 0)
! {
! PLAYER_ERROR ("unable to subscribe to camera device");
return -1;
}
***************
*** 292,296 ****
{
// Unsubscribe from devices.
! this->position->Unsubscribe(this);
return 0;
--- 281,285 ----
{
// Unsubscribe from devices.
! this->position->Unsubscribe(InQueue);
return 0;
***************
*** 452,480 ****
// Process any pending requests.
! HandleRequests();
// Update the InertiaCube
UpdateImu();
-
- // See if there is any new position data. If there is, generate a
- // new pose estimate.
- if (UpdatePosition())
- {
- // Generate a new pose estimate.
- UpdatePose();
-
- // TESTING
- printf("%.3f %.3f %.0f : ",
- this->position_new_pose[0],
- this->position_new_pose[1],
- this->position_new_pose[2] * 180 / M_PI);
- printf("%.3f %.3f %.0f \r",
- this->pose[0],
- this->pose[1],
- this->pose[2] * 180 / M_PI);
-
- // Expose the new estimate to the server.
- UpdateData();
- }
}
return;
--- 441,448 ----
// Process any pending requests.
! ProcessMessages();
// Update the InertiaCube
UpdateImu();
}
return;
***************
*** 483,559 ****
////////////////////////////////////////////////////////////////////////////////
! // Process requests. Returns 1 if the configuration has changed.
! int InertiaCube2::HandleRequests()
{
! int len;
! void *client;
! char request[PLAYER_MAX_REQREP_SIZE];
! while ((len = GetConfig(&client, &request, sizeof(request))) > 0)
{
! switch (request[0])
! {
! /* TODO
! case PLAYER_POSITION_GET_GEOM_REQ:
! HandleGetGeom(client, request, len);
! break;
! */
!
! default:
! if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
! PLAYER_ERROR("PutReply() failed");
! break;
! }
}
! return 0;
}
-
////////////////////////////////////////////////////////////////////////////////
- // Handle geometry requests.
- void InertiaCube2::HandleGetGeom(void *client, void *request, int len)
- {
- /* TODO
- player_device_id_t id;
- uint8_t req;
- player_position_geom_t geom;
- struct timeval ts;
- uint16_t reptype;
-
- if (len != 1)
- {
- PLAYER_ERROR2("geometry request len is invalid (%d != %d)", len, 1);
- if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
- PLAYER_ERROR("PutReply() failed");
- return;
- }
-
- id.code = PLAYER_POSITION_CODE;
- id.index = this->position_index;
- id.port = this->device_id.port;
-
- // Get underlying device geometry.
- req = PLAYER_POSITION_GET_GEOM_REQ;
- if (this->Request(&id, this, &req, 1, &reptype, &ts, &geom, sizeof(geom))
!= 0)
- {
- PLAYER_ERROR("unable to get position device geometry");
- if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
- PLAYER_ERROR("PutReply() failed");
- return;
- }
- if (reptype != PLAYER_MSGTYPE_RESP_ACK)
- {
- if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
- PLAYER_ERROR("PutReply() failed");
- return;
- }
-
- if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &geom, sizeof(geom)) !=
0)
- PLAYER_ERROR("PutReply() failed");
- */
-
- return;
- }
-
////////////////////////////////////////////////////////////////////////////////
--- 451,472 ----
////////////////////////////////////////////////////////////////////////////////
! // Process an incoming message
! int InertiaCube2::ProcessMessage (MessageQueue * resp_queue, player_msghdr *
hdr, void * data)
{
! assert(hdr);
! assert(data);
! if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA ,
PLAYER_POSITION2D_DATA_STATE, position_id))
{
! player_position2d_data_t & pos_data =
*reinterpret_cast<player_position2d_data_t *> (data);
! position_new_pose = pos_data.pos;
! UpdatePose();
! UpdateData();
! return 0;
}
! return -1;
}
////////////////////////////////////////////////////////////////////////////////
***************
*** 584,618 ****
-
////////////////////////////////////////////////////////////////////////////////
- // Update the position device (returns non-zero if changed).
- int InertiaCube2::UpdatePosition()
- {
- int i;
- size_t size;
- player_position_data_t data;
- uint32_t timesec, timeusec;
- double time;
-
- // Get the position device data.
- size = this->position->GetData(this,(unsigned char*) &data, sizeof(data),
×ec, &timeusec);
- time = (double) timesec + ((double) timeusec) * 1e-6;
-
- // Dont do anything if this is old data.
- if (time - this->position_time < 0.001)
- return 0;
- this->position_time = time;
-
- // Byte swap
- data.xpos = ntohl(data.xpos);
- data.ypos = ntohl(data.ypos);
- data.yaw = ntohl(data.yaw);
-
- this->position_new_pose[0] = (double) data.xpos / 1000.0;
- this->position_new_pose[1] = (double) data.ypos / 1000.0;
- this->position_new_pose[2] = (double) data.yaw * M_PI / 180;
-
- return 1;
- }
-
////////////////////////////////////////////////////////////////////////////////
--- 497,500 ----
***************
*** 626,643 ****
// Compute change in pose relative to previous pose.
! dx = this->position_new_pose[0] - this->position_old_pose[0];
! dy = this->position_new_pose[1] - this->position_old_pose[1];
! da = this->position_old_pose[2];
tx = dx * cos(da) + dy * sin(da);
ty = -dx * sin(da) + dy * cos(da);
// Add this to the previous pose esimate.
! this->pose[0] += tx * cos(this->imu_old_orient) - ty *
sin(this->imu_old_orient);
! this->pose[1] += tx * sin(this->imu_old_orient) + ty *
cos(this->imu_old_orient);
! this->pose[2] = this->imu_new_orient;
! this->position_old_pose[0] = this->position_new_pose[0];
! this->position_old_pose[1] = this->position_new_pose[1];
! this->position_old_pose[2] = this->position_new_pose[2];
this->imu_old_orient = this->imu_new_orient;
--- 508,523 ----
// Compute change in pose relative to previous pose.
! dx = this->position_new_pose.px - this->position_old_pose.px;
! dy = this->position_new_pose.py - this->position_old_pose.py;
! da = this->position_old_pose.pa;
tx = dx * cos(da) + dy * sin(da);
ty = -dx * sin(da) + dy * cos(da);
// Add this to the previous pose esimate.
! this->pose.px += tx * cos(this->imu_old_orient) - ty *
sin(this->imu_old_orient);
! this->pose.py += tx * sin(this->imu_old_orient) + ty *
cos(this->imu_old_orient);
! this->pose.pa = this->imu_new_orient;
! this->position_old_pose = this->position_new_pose;
this->imu_old_orient = this->imu_new_orient;
***************
*** 651,671 ****
{
uint32_t timesec, timeusec;
! player_position_data_t data;
!
! data.xpos = (int32_t) (this->pose[0] * 1000);
! data.ypos = (int32_t) (this->pose[1] * 1000);
! data.yaw = (int32_t) (this->pose[2] * 180 / M_PI);
!
! // Byte swap
! data.xpos = htonl(data.xpos);
! data.ypos = htonl(data.ypos);
! data.yaw = htonl(data.yaw);
! // Compute time. Use the position device's time.
! timesec = (uint32_t) this->position_time;
! timeusec = (uint32_t) (fmod(this->position_time, 1.0) * 1e6);
// Copy data to server.
! PutData((unsigned char*) &data, sizeof(data), timesec, timeusec);
return;
--- 531,540 ----
{
uint32_t timesec, timeusec;
! player_position2d_data_t data;
! data.pos = pose;
// Copy data to server.
! Publish(device_addr, NULL, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE, (unsigned char*) &data, sizeof(data),
&position_time);
return;
Index: Makefile.am
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/isense/Makefile.am,v
retrieving revision 1.1
retrieving revision 1.1.16.1
diff -C2 -d -r1.1 -r1.1.16.1
*** Makefile.am 29 Nov 2002 17:08:13 -0000 1.1
--- Makefile.am 7 Jun 2006 16:12:52 -0000 1.1.16.1
***************
*** 1,7 ****
! AM_CPPFLAGS = -I$(top_srcdir)/server
!
! noinst_LIBRARIES = @ISENSE_LIB@
! EXTRA_LIBRARIES = libisense.a
! libisense_a_SOURCES = inertiacube2.cc
--- 1,8 ----
! AM_CPPFLAGS = -Wall -I$(top_srcdir)
! noinst_LTLIBRARIES =
! if INCLUDE_ISENSE
! noinst_LTLIBRARIES += libisense.la
! endif
! libisense_la_SOURCES = inertiacube2.cc
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
