[ROS] iri_dynamixel_pan_tilt -> Added a service to go to a default position.

CMakeLists.txt

@@ -35,6 +35,7 @@ add_service_files(
   FILES
   setMode.srv
   getPosition.srv
+  defaultPosition.srv
 #  setComplianceControl.srv
 #  setPIDControl.srv
 #  getPIDControl.srv

cfg/DynamixelPanTilt.cfg

@@ -48,8 +48,8 @@ mode_enum = gen.enum([ gen.const("absolute_angle", int_t, 0, "Absolute angle con
 
 gen.add("mode", int_t, SensorLevels.RECONFIGURE_RUNNING, "The control mode which is edited via an enum", 0, 0, 2, edit_method=mode_enum)
 
-gen.add("pan_tilt_configuration_file", str_t, SensorLevels.RECONFIGURE_CLOSE, "The xml configuration file")
-gen.add("server_configuration_file", str_t, SensorLevels.RECONFIGURE_CLOSE, "The xml  configuration file")
+gen.add("pan_tilt_configuration_file", str_t, SensorLevels.RECONFIGURE_CLOSE, "The pant-tilt xml configuration file")
+gen.add("server_configuration_file", str_t, SensorLevels.RECONFIGURE_CLOSE, "The server xml configuration file")
 
 gen.add("pan_tilt_name", str_t, SensorLevels.RECONFIGURE_CLOSE, "The pan&tilt human friendly name", "PAN-TILT-AX-12+")
 gen.add("pan_id", int_t, SensorLevels.RECONFIGURE_CLOSE, "The pan ID", 1, 0, 253)

config/dyn_server_ftdi_config.xml

@@ -4,5 +4,5 @@
        xsi:noNamespaceSchemaLocation="dyn_server_ftdi_cfg_file.xsd">
 
   <baudrate>1000000</baudrate>
-  <serial_num>AI027ZIZ</serial_num>
+  <serial_num>A4012AD3</serial_num>
 </dyn_server_ftdi_config>

include/dynamixel_pan_tilt_driver_node.h

@@ -39,6 +39,7 @@
 #include <iri_common_drivers_msgs/setPIDControl.h>
 #include <iri_common_drivers_msgs/setComplianceControl.h>
 #include <iri_dynamixel_pan_tilt/setMode.h>
+#include <iri_dynamixel_pan_tilt/defaultPosition.h>
 
 // [action server client headers]
 #include <iri_action_server/iri_action_server.h>
@@ -139,6 +140,11 @@ class DynamixelPanTiltDriverNode : public iri_base_driver::IriBaseNodeDriver<Dyn
     void set_mode_mutex_enter(void);
     void set_mode_mutex_exit(void);
 
+    ros::ServiceServer default_position_server_;
+    bool default_positionCallback(iri_dynamixel_pan_tilt::defaultPosition::Request &req, iri_dynamixel_pan_tilt::defaultPosition::Response &res);
+    pthread_mutex_t default_position_mutex_;
+    void default_position_mutex_enter(void);
+    void default_position_mutex_exit(void);
 
     // [client attributes]
 

src/dynamixel_pan_tilt_driver.cpp

@@ -76,7 +76,7 @@ bool DynamixelPanTiltDriver::stopDriver(void)
 
 void DynamixelPanTiltDriver::config_update(Config& new_cfg, uint32_t level)
 {
-  this->lock();
+  lock();
   
   // depending on current state
   // update driver with new_cfg data
@@ -98,7 +98,7 @@ void DynamixelPanTiltDriver::config_update(Config& new_cfg, uint32_t level)
   // save the current configuration
   this->mode = (Dynamixel_pan_tilt_mode)new_cfg.mode;
   this->config_=new_cfg;
-  this->unlock();
+  unlock();
 }
 
 DynamixelPanTiltDriver::~DynamixelPanTiltDriver(void)

src/dynamixel_pan_tilt_driver_node.cpp

@@ -858,6 +858,45 @@ void DynamixelPanTiltDriverNode::set_mode_mutex_exit(void)
   pthread_mutex_unlock(&this->set_mode_mutex_);
 }
 
+bool DynamixelPanTiltDriverNode::default_positionCallback(iri_dynamixel_pan_tilt::defaultPosition::Request &req, iri_dynamixel_pan_tilt::defaultPosition::Response &res)
+{
+  if (ros::ok())
+  {
+    ROS_DEBUG("DynamixelPanTiltDriverNode::default_positionCallback: New Request Received!");
+    try
+    {
+      this->driver_.set_mode((Dynamixel_pan_tilt_mode) 0);
+      Dynamixel_pan_tilt_data angle, sp, pos;
+      angle.pan = req.default_pan;
+      angle.tilt = req.default_tilt;
+      sp.pan = req.pan_speed;
+      sp.tilt = req.tilt_speed;
+      this->driver_.move_absolute_angle(angle, sp);
+      pos = this->driver_.get_position();
+      while (fabs(pos.pan - req.default_pan) > 2 || fabs(pos.tilt - req.default_tilt) > 2)
+        ros::Duration(0.1).sleep();
+      this->driver_.set_mode((Dynamixel_pan_tilt_mode) 0);
+      res.success = true;
+    }
+    catch (CDynamixel_Pan_TiltException &e)
+    {
+      ROS_ERROR("Something wrong happenned while moving to the default position.");
+      res.success = false;
+    }
+  }
+  return true;
+}
+
+void DynamixelPanTiltDriverNode::default_position_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->default_position_mutex_);
+}
+
+void DynamixelPanTiltDriverNode::default_position_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->default_position_mutex_);
+}
+
 
 /*  [action callbacks] */
 void DynamixelPanTiltDriverNode::scanStartCallback(const iri_dynamixel_pan_tilt::scanGoalConstPtr& goal)