Added sensor pose read & write services

stdr_msgs/CMakeLists.txt

@@ -59,6 +59,9 @@ add_service_files(
 
     AddCO2Source.srv
     DeleteCO2Source.srv
+
+    ReadSensorPose.srv
+    WriteSensorPose.srv
 )
 
 add_action_files(

stdr_msgs/msg/SensorPoseMsg.msg

@@ -0,0 +1,2 @@
+string frame_id
+geometry_msgs/Pose2D pose # sensor pose, relative to robot center

stdr_msgs/srv/ReadSensorPose.srv

@@ -0,0 +1,2 @@
+---
+geometry_msgs/Pose2D pose # sensor pose, relative to robot center

stdr_msgs/srv/WriteSensorPose.srv

@@ -0,0 +1,2 @@
+geometry_msgs/Pose2D pose # sensor pose, relative to robot center
+---

stdr_robot/include/stdr_robot/sensors/sensor_base.h

@@ -26,6 +26,8 @@
 #include <tf/transform_listener.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <geometry_msgs/Pose2D.h>
+#include <stdr_msgs/ReadSensorPose.h>
+#include <stdr_msgs/WriteSensorPose.h>
 
 /**
 @namespace stdr_robot
@@ -52,11 +54,20 @@ namespace stdr_robot {
       @brief Getter function for returning the sensor pose relatively to robot
       @return geometry_msgs::Pose2D
       **/ 
-      inline geometry_msgs::Pose2D getSensorPose(void) const
+      inline geometry_msgs::Pose2D getSensorPose(void)
       {
         return _sensorPose;
       }
 
+      /**
+      @brief Setter function for changing the sensor pose relatively to robot
+      @return void
+      **/ 
+      inline void setSensorPose(geometry_msgs::Pose2D& pose)
+      {
+        _sensorPose = pose;
+      }
+
       /**
       @brief Getter function for returning the sensor frame id
       @return std::string
@@ -107,6 +118,24 @@ namespace stdr_robot {
       @return void
       **/ 
       void updateTransform(const ros::TimerEvent& ev);
+
+      /**
+      @brief The callback of the read robot sensor pose service
+      @param req [stdr_msgs::ReadSensorPose::Request&] The service request
+      @param res [stdr_msgs::ReadSensorPose::Response&] The service result
+      @return bool
+      **/
+      bool readSensorPoseCallback(stdr_msgs::ReadSensorPose::Request& req,
+				  stdr_msgs::ReadSensorPose::Response& res);
+
+      /**
+      @brief The callback of the write robot sensor pose service
+      @param req [stdr_msgs::WriteSensorPose::Request&] The service request
+      @param res [stdr_msgs::WriteSensorPose::Response&] The service result
+      @return bool
+      **/
+      bool writeSensorPoseCallback(stdr_msgs::WriteSensorPose::Request& req,
+				  stdr_msgs::WriteSensorPose::Response& res);
 
     protected:
 
@@ -116,7 +145,7 @@ namespace stdr_robot {
       const nav_msgs::OccupancyGrid& _map;
 
       //!< Sensor pose relative to robot
-      const geometry_msgs::Pose2D _sensorPose;
+      geometry_msgs::Pose2D _sensorPose;
       //!< Update frequency of _timer
       const float _updateFrequency;
       //!< Sensor frame id
@@ -136,6 +165,11 @@ namespace stdr_robot {
 
       //!< True if sensor got the _sensorTransform
       bool _gotTransform;
+
+      //!< ROS service servers to read & write sensor pose:
+      ros::ServiceServer _readSensorPoseService;
+      ros::ServiceServer _writeSensorPoseService;
+
   };
 
   typedef boost::shared_ptr<Sensor> SensorPtr;

stdr_robot/src/sensors/sensor_base.cpp

@@ -52,6 +52,13 @@ namespace stdr_robot {
       ros::Duration(1/updateFrequency), &Sensor::checkAndUpdateSensor, this);
     _tfTimer = n.createTimer(
       ros::Duration(1/(2*updateFrequency)), &Sensor::updateTransform, this);
+
+    _readSensorPoseService = n.advertiseService(_namespace + "/" + _sensorFrameId + "/" + "readpose",
+						&Sensor::readSensorPoseCallback, this);
+
+    _writeSensorPoseService = n.advertiseService(_namespace + "/" + _sensorFrameId + "/" + "writepose",
+						&Sensor::writeSensorPoseCallback, this);
+
   }
 
 
@@ -89,4 +96,31 @@ namespace stdr_robot {
       ROS_DEBUG("%s",ex.what());
     }
   }
+
+  /**
+  @brief The callback of the read robot sensor pose service
+  @param req [stdr_msgs::ReadSensorPose::Request&] The service request
+  @param res [stdr_msgs::ReadSensorPose::Response&] The service result
+  @return bool
+  **/
+  bool Sensor::readSensorPoseCallback(stdr_msgs::ReadSensorPose::Request& req,
+                stdr_msgs::ReadSensorPose::Response& res)
+  {
+    res.pose = getSensorPose();
+    return true;
+  }
+
+  /**
+  @brief The callback of the write robot sensor pose service
+  @param req [stdr_msgs::WriteSensorPose::Request&] The service request
+  @param res [stdr_msgs::WriteSensorPose::Response&] The service result
+  @return bool
+  **/
+  bool Sensor::writeSensorPoseCallback(stdr_msgs::WriteSensorPose::Request& req,
+                stdr_msgs::WriteSensorPose::Response& res)
+  {
+    setSensorPose(req.pose);
+    return true;
+  }
+
 }  // namespace stdr_robot