Update of /cvsroot/playerstage/code/player/server/drivers/localization/amcl
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv19732/server/drivers/localization/amcl
Modified Files:
Tag: release-2-0-patches
amcl.cc amcl.h amcl_laser.cc amcl_laser.h amcl_odom.cc
amcl_odom.h amcl_sensor.cc amcl_sensor.h
Log Message:
merged amcl changes from HEAD to enable geometry response and position2d output
Index: amcl.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/localization/amcl/amcl.cc,v
retrieving revision 1.70.2.1
retrieving revision 1.70.2.2
diff -C2 -d -r1.70.2.1 -r1.70.2.2
*** amcl.cc 7 Jun 2006 16:12:48 -0000 1.70.2.1
--- amcl.cc 13 Jul 2006 17:59:43 -0000 1.70.2.2
***************
*** 242,250 ****
(
name "p2os_position"
- <<<<<<< amcl.cc
- provides ["odometry:::position:0"]
- =======
provides ["odometry:::position2d:0"]
- >>>>>>> 1.72
port "/dev/ttyS0"
)
--- 242,246 ----
***************
*** 660,665 ****
void AdaptiveMCL::Main(void)
{
- struct timespec sleeptime;
-
this->q_len = 0;
--- 656,659 ----
***************
*** 700,705 ****
// Sleep for 1ms (will actually take longer than this).
! sleeptime.tv_sec = 0;
! sleeptime.tv_nsec = 1000000L;
nanosleep(&sleeptime, NULL);
--- 694,698 ----
// Sleep for 1ms (will actually take longer than this).
! const struct timespec sleeptime = {0, 1000000L};
nanosleep(&sleeptime, NULL);
***************
*** 959,963 ****
// Encode data to send to server
this->PutDataLocalize(ts);
! //this->PutDataPosition(ts, delta);
return true;
--- 952,956 ----
// Encode data to send to server
this->PutDataLocalize(ts);
! this->PutDataPosition(delta);
return true;
***************
*** 978,986 ****
}
- #if 0
// Encode data to send to server; only the position interface
! // gets updates every cycle
! this->PutDataPosition(ts, delta);
! #endif
return false;
--- 971,977 ----
}
// Encode data to send to server; only the position interface
! // gets updated every cycle
! this->PutDataPosition(delta);
return false;
***************
*** 1188,1191 ****
--- 1179,1188 ----
(void*)&resp, sizeof(resp), NULL);
return(0);
+ } else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_POSITION2D_REQ_GET_GEOM,
device_addr))
+ {
+ assert(hdr->size == 0);
+ ProcessGeom(resp_queue, hdr);
+ return(0);
}
***************
*** 1201,1204 ****
--- 1198,1218 ----
}
+ void
+ AdaptiveMCL::ProcessGeom(MessageQueue* resp_queue, player_msghdr_t* hdr)
+ {
+ player_position2d_geom_t geom;
+ // just return a point so we don't get errors from playerv
+ geom.pose.px = 0;
+ geom.pose.py = 0;
+ geom.pose.pa = 0;
+ geom.size.sl = 0.01;
+ geom.size.sw = 0.01;
+
+ Publish(this->position_addr, resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_POSITION2D_REQ_GET_GEOM,
+ &geom, sizeof(geom), NULL);
+ }
+
#ifdef INCLUDE_RTKGUI
Index: amcl.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/localization/amcl/amcl.h,v
retrieving revision 1.13.2.1
retrieving revision 1.13.2.2
diff -C2 -d -r1.13.2.1 -r1.13.2.2
*** amcl.h 7 Jun 2006 16:12:48 -0000 1.13.2.1
--- amcl.h 13 Jul 2006 17:59:43 -0000 1.13.2.2
***************
*** 61,65 ****
// Covariance of pose estimate
pf_matrix_t pf_pose_cov;
!
} amcl_hyp_t;
--- 61,65 ----
// Covariance of pose estimate
pf_matrix_t pf_pose_cov;
!
} amcl_hyp_t;
***************
*** 83,87 ****
public: virtual int Setup(void);
public: virtual int Shutdown(void);
!
///////////////////////////////////////////////////////////////////////////
// Middle methods: these methods facilitate communication between the top
--- 83,87 ----
public: virtual int Setup(void);
public: virtual int Shutdown(void);
!
///////////////////////////////////////////////////////////////////////////
// Middle methods: these methods facilitate communication between the top
***************
*** 103,113 ****
// MessageHandler
! public: virtual int ProcessMessage(MessageQueue * resp_queue,
! player_msghdr * hdr,
void * data);
// Check for updated sensor data
public: virtual void UpdateSensorData(void);
!
// Main function for device thread.
private: virtual void Main(void);
--- 103,113 ----
// MessageHandler
! public: virtual int ProcessMessage(MessageQueue * resp_queue,
! player_msghdr * hdr,
void * data);
// Check for updated sensor data
public: virtual void UpdateSensorData(void);
!
// Main function for device thread.
private: virtual void Main(void);
***************
*** 128,131 ****
--- 128,134 ----
private: void PutDataPosition(pf_vector_t delta);
+ // Send back geometry data
+ private: void ProcessGeom(MessageQueue* resp_queue, player_msghdr_t* hdr);
+
#ifdef INCLUDE_RTKGUI
// Set up the GUI
***************
*** 149,153 ****
private: player_devaddr_t position_addr;
private: player_devaddr_t localize_addr;
!
// List of all sensors
private: int sensor_count;
--- 152,156 ----
private: player_devaddr_t position_addr;
private: player_devaddr_t localize_addr;
!
// List of all sensors
private: int sensor_count;
***************
*** 168,172 ****
private: int q_size, q_start, q_len;
private: AMCLSensorData **q_data;
!
// Current particle filter pose estimates
private: int hyp_count;
--- 171,175 ----
private: int q_size, q_start, q_len;
private: AMCLSensorData **q_data;
!
// Current particle filter pose estimates
private: int hyp_count;
-------------------------------------------------------------------------
Using Tomcat but need to do more? Need to support web services, security?
Get stuff done quickly with pre-integrated technology to make your job easier
Download IBM WebSphere Application Server v.1.0.1 based on Apache Geronimo
http://sel.as-us.falkag.net/sel?cmd=lnk&kid=120709&bid=263057&dat=121642
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
