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