Revision: 7212
http://playerstage.svn.sourceforge.net/playerstage/?rev=7212&view=rev
Author: natepak
Date: 2008-12-22 18:10:00 +0000 (Mon, 22 Dec 2008)
Log Message:
-----------
Implemented RequestGeom for the laser interface
Modified Paths:
--------------
code/gazebo/trunk/examples/player/laser/laser.cc
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/player/LaserInterface.cc
code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
code/gazebo/trunk/server/sensors/Sensor.cc
code/gazebo/trunk/server/sensors/Sensor.hh
Modified: code/gazebo/trunk/examples/player/laser/laser.cc
===================================================================
--- code/gazebo/trunk/examples/player/laser/laser.cc 2008-12-19 08:11:32 UTC
(rev 7211)
+++ code/gazebo/trunk/examples/player/laser/laser.cc 2008-12-22 18:10:00 UTC
(rev 7212)
@@ -16,10 +16,20 @@
// Subscribe to the laser proxy
LaserProxy lp(&robot, 0);
+ robot.Read();
+
// Print out some stuff
std::cout << robot << std::endl;
- for (;;)
+ lp.RequestGeom();
+
+ player_pose3d_t laserPose = lp.GetPose();
+
+ // Print out laser stuff
+ std::cout << "Laser Pose[" << laserPose.px << " " <<
+ laserPose.py << " " << laserPose.pyaw << "]\n";
+
+ /*for (;;)
{
// This blocks until new data comes
robot.Read();
@@ -29,7 +39,7 @@
printf("[%f %d]", lp.GetRange(i), lp.GetIntensity(i) );
}
printf("\n");
- }
+ }*/
}
catch (PlayerCc::PlayerError e)
{
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2008-12-19 08:11:32 UTC (rev
7211)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2008-12-22 18:10:00 UTC (rev
7212)
@@ -852,6 +852,12 @@
/// Commaned range count
public: int cmd_range_count;
+
+ /// Pose of the laser
+ public: Pose pose;
+
+ /// Size of the laser
+ public: Vec3 size;
};
/// \brief Laser interface
Modified: code/gazebo/trunk/player/LaserInterface.cc
===================================================================
--- code/gazebo/trunk/player/LaserInterface.cc 2008-12-19 08:11:32 UTC (rev
7211)
+++ code/gazebo/trunk/player/LaserInterface.cc 2008-12-22 18:10:00 UTC (rev
7212)
@@ -25,7 +25,6 @@
*/
/* TODO
-PLAYER_LASER_REQ_GET_GEOM
*/
#include <math.h>
@@ -79,7 +78,7 @@
PLAYER_LASER_REQ_SET_CONFIG,
this->device_addr))
{
- player_laser_config_t* plc = (player_laser_config_t*)data;
+ //player_laser_config_t* plc = (player_laser_config_t*)data;
if ( hdr->size == sizeof(player_laser_config_t) )
{
@@ -137,15 +136,18 @@
{
player_laser_geom_t rep;
+
// TODO: get geometry from somewhere
memset(&rep.pose, 0, sizeof(rep.pose));
memset(&rep.size, 0, sizeof(rep.size));
-// rep.pose.px = 0.0;
-// rep.pose.py = 0.0;
-// rep.pose.pyaw = 0.0;
-// rep.size.sw = 0.0;
-// rep.size.sl = 0.0;
+ rep.pose.px = this->iface->data->pose.pos.x;
+ rep.pose.py = this->iface->data->pose.pos.y;
+ rep.pose.pyaw = this->iface->data->pose.yaw;
+
+ rep.size.sw = this->iface->data->size.x;
+ rep.size.sl = this->iface->data->size.y;
+
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LASER_REQ_GET_GEOM,
Modified:
code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
===================================================================
--- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
2008-12-19 08:11:32 UTC (rev 7211)
+++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
2008-12-22 18:10:00 UTC (rev 7212)
@@ -123,6 +123,23 @@
{
this->myParent->SetActive(false);
}
+
+ Quatern rot = this->myParent->GetPose().rot;
+ Vector3 pos = this->myParent->GetPose().pos;
+
+ // Update the pose
+ this->laserIface->data->pose.pos.x = pos.x;
+ this->laserIface->data->pose.pos.y = pos.y;
+ this->laserIface->data->pose.pos.z = pos.z;
+
+ this->laserIface->data->pose.roll = rot.GetRoll();
+ this->laserIface->data->pose.pitch = rot.GetPitch();
+ this->laserIface->data->pose.yaw = rot.GetYaw();
+
+ this->laserIface->data->size.x = 0.1;
+ this->laserIface->data->size.y = 0.1;
+ this->laserIface->data->size.z = 0.1;
+
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/sensors/Sensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/Sensor.cc 2008-12-19 08:11:32 UTC (rev
7211)
+++ code/gazebo/trunk/server/sensors/Sensor.cc 2008-12-22 18:10:00 UTC (rev
7212)
@@ -185,4 +185,9 @@
this->active = value;
}
-
+////////////////////////////////////////////////////////////////////////////////
+/// Get the current pose
+const Pose3d &Sensor::GetPose() const
+{
+ return this->body->GetPose();
+}
Modified: code/gazebo/trunk/server/sensors/Sensor.hh
===================================================================
--- code/gazebo/trunk/server/sensors/Sensor.hh 2008-12-19 08:11:32 UTC (rev
7211)
+++ code/gazebo/trunk/server/sensors/Sensor.hh 2008-12-22 18:10:00 UTC (rev
7212)
@@ -28,6 +28,7 @@
#define SENSOR_HH
#include "Param.hh"
+#include "Pose3d.hh"
#include "Entity.hh"
namespace gazebo
@@ -68,6 +69,9 @@
/// \brief Finalize the sensor
public: void Fini();
+ /// \brief Get the current pose
+ public: const Pose3d &GetPose() const;
+
/// \brief Set whether the sensor is active or not
public: void SetActive(bool value);
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
