Changed the class name of the IR sensor driver wrapper.

ir_feet_sensor/CMakeLists.txt

 cmake_minimum_required(VERSION 2.8.3)
-project(ir_feet_sensor)
+project(ir_foot_sensor)
 
 ## Find catkin macros and libraries
 find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_driver)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver humanoid_common_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -17,7 +17,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_driver)
 # find_package(<dependency> REQUIRED)
 find_package(iriutils REQUIRED)
 find_package(comm REQUIRED)
-find_package(iriutils REQUIRED)
+find_package(dynamixel REQUIRED)
 find_package(ir_feet REQUIRED)
 
 # ******************************************************************** 
@@ -64,7 +64,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_driver
+ CATKIN_DEPENDS iri_base_driver humanoid_common_msgs
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -91,7 +91,7 @@ include_directories(${ir_feet_INCLUDE_DIR})
 # add_library(${PROJECT_NAME} <list of source files>)
 
 ## Declare a cpp executable
-add_executable(${PROJECT_NAME} src/ir_feet_sensor_driver.cpp src/ir_feet_sensor_driver_node.cpp)
+add_executable(${PROJECT_NAME} src/ir_foot_sensor_driver.cpp src/ir_foot_sensor_driver_node.cpp)
 
 # ******************************************************************** 
 #                   Add the libraries
@@ -107,6 +107,7 @@ target_link_libraries(${PROJECT_NAME} ${ir_feet_LIBRARY})
 #               Add message headers dependencies 
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} humanoid_common_msgs_generate_messages_cpp)
 # ******************************************************************** 
 #               Add dynamic reconfigure dependencies 
 # ******************************************************************** 

ir_feet_sensor/cfg/IrFeetSensor.cfg → ir_feet_sensor/cfg/IrFootSensor.cfg

@@ -31,7 +31,7 @@
 
 # Author: 
 
-PACKAGE='ir_feet_sensor'
+PACKAGE='ir_foot_sensor'
 
 from driver_base.msg import SensorLevels
 from dynamic_reconfigure.parameter_generator_catkin import *
@@ -44,4 +44,4 @@ gen.add("dyn_serial",               str_t,     SensorLevels.RECONFIGURE_STOP,
 gen.add("dyn_baudrate",             int_t,     SensorLevels.RECONFIGURE_STOP,   "Dynamixel serial baudrate",              1000000,  9600, 3000000)
 gen.add("ir_foot_id",               int_t,     SensorLevels.RECONFIGURE_STOP,   "IR foot identifier",            1,        0,    254)
 
-exit(gen.generate(PACKAGE, "IrFeetSensorDriver", "IrFeetSensor"))
+exit(gen.generate(PACKAGE, "IrFootSensorDriver", "IrFootSensor"))

ir_feet_sensor/include/ir_feet_sensor_driver.h → ir_feet_sensor/include/ir_foot_sensor_driver.h

@@ -22,13 +22,13 @@
 // refer to the IRI wiki page for more information:
 // http://wikiri.upc.es/index.php/Robotics_Lab
 
-#ifndef _ir_feet_sensor_driver_h_
-#define _ir_feet_sensor_driver_h_
+#ifndef _ir_foot_sensor_driver_h_
+#define _ir_foot_sensor_driver_h_
 
 #include <iri_base_driver/iri_base_driver.h>
-#include <ir_feet_sensor/IrFeetSensorConfig.h>
+#include <ir_foot_sensor/IrFootSensorConfig.h>
 
-//include ir_feet_sensor_driver main library
+//include ir_foot_sensor_driver main library
 #include "dynamixelserver_serial.h"
 #include "ir_feet.h"
 
@@ -41,29 +41,29 @@
  * ROS driver structure. ROS driver_base state transitions are already managed
  * by IriBaseDriver.
  *
- * The IrFeetSensorDriver class must implement all specific driver requirements to
+ * The IrFootSensorDriver class must implement all specific driver requirements to
  * safetely open, close, run and stop the driver at any time. It also must 
  * guarantee an accessible interface for all driver's parameters.
  *
- * The IrFeetSensorConfig.cfg needs to be filled up with those parameters suitable
+ * The IrFootSensorConfig.cfg needs to be filled up with those parameters suitable
  * to be changed dynamically by the ROS dyanmic reconfigure application. The 
  * implementation of the CIriNode class will manage those parameters through
  * methods like postNodeOpenHook() and reconfigureNodeHook().
  *
  */
-class IrFeetSensorDriver : public iri_base_driver::IriBaseDriver
+class IrFootSensorDriver : public iri_base_driver::IriBaseDriver
 {
   private:
     // private attributes and methods
-    CIRFeet<CDynamixelServerSerial> *device;
+    CIRFoot<CDynamixelServerSerial> *device;
   public:
    /**
     * \brief define config type
     *
-    * Define a Config type with the IrFeetSensorConfig. All driver implementations
+    * Define a Config type with the IrFootSensorConfig. All driver implementations
     * will then use the same variable type Config.
     */
-    typedef ir_feet_sensor::IrFeetSensorConfig Config;
+    typedef ir_foot_sensor::IrFootSensorConfig Config;
 
    /**
     * \brief config variable
@@ -81,7 +81,7 @@ class IrFeetSensorDriver : public iri_base_driver::IriBaseDriver
     * Attributes from the main node driver class IriBaseDriver such as loop_rate,
     * may be also overload here.
     */
-    IrFeetSensorDriver(void);
+    IrFootSensorDriver(void);
 
    /**
     * \brief open driver
@@ -144,7 +144,7 @@ class IrFeetSensorDriver : public iri_base_driver::IriBaseDriver
     */
     void config_update(Config& new_cfg, uint32_t level=0);
 
-    // here define all ir_feet_sensor_driver interface methods to retrieve and set
+    // here define all ir_foot_sensor_driver interface methods to retrieve and set
     // the driver parameters
     std::vector<double> get_all_sensor_voltages(void);
     std::vector<double> get_all_sensor_thresholds(void);
@@ -157,7 +157,7 @@ class IrFeetSensorDriver : public iri_base_driver::IriBaseDriver
     * This destructor is called when the object is about to be destroyed.
     *
     */
-    ~IrFeetSensorDriver(void);
+    ~IrFootSensorDriver(void);
 };
 
 #endif

ir_feet_sensor/include/ir_feet_sensor_driver_node.h → ir_feet_sensor/include/ir_foot_sensor_driver_node.h

@@ -22,13 +22,14 @@
 // refer to the IRI wiki page for more information:
 // http://wikiri.upc.es/index.php/Robotics_Lab
 
-#ifndef _ir_feet_sensor_driver_node_h_
-#define _ir_feet_sensor_driver_node_h_
+#ifndef _ir_foot_sensor_driver_node_h_
+#define _ir_foot_sensor_driver_node_h_
 
 #include <iri_base_driver/iri_base_driver_node.h>
-#include "ir_feet_sensor_driver.h"
+#include "ir_foot_sensor_driver.h"
 
 // [publisher subscriber headers]
+#include <humanoid_common_msgs/ir_foot_data.h>
 
 // [service client headers]
 
@@ -51,10 +52,13 @@
  * http://www.ros.org/wiki/diagnostics/ (Tutorials: Creating a Diagnostic Analyzer)
  * http://www.ros.org/wiki/self_test/ (Example: Self Test)
  */
-class IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetSensorDriver>
+class IrFootSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>
 {
   private:
     // [publisher attributes]
+    ros::Publisher sensor_data_publisher_;
+    humanoid_common_msgs::ir_foot_data sensor_data_ir_foot_data_msg_;
+
 
     // [subscriber attributes]
 
@@ -79,7 +83,7 @@ class IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetS
    /**
     * \brief constructor
     *
-    * This constructor mainly creates and initializes the IrFeetSensorDriverNode topics
+    * This constructor mainly creates and initializes the IrFootSensorDriverNode topics
     * through the given public_node_handle object. IriBaseNodeDriver attributes 
     * may be also modified to suit node specifications.
     *
@@ -93,7 +97,7 @@ class IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetS
     *
     * \param nh a reference to the node handle object to manage all ROS topics.
     */
-    IrFeetSensorDriverNode(ros::NodeHandle& nh);
+    IrFootSensorDriverNode(ros::NodeHandle& nh);
 
    /**
     * \brief Destructor
@@ -101,7 +105,7 @@ class IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetS
     * This destructor is called when the object is about to be destroyed.
     *
     */
-    ~IrFeetSensorDriverNode(void);
+    ~IrFootSensorDriverNode(void);
 
   protected:
    /**

ir_feet_sensor/package.xml

 <?xml version="1.0"?>
 <package>
-  <name>ir_feet_sensor</name>
+  <name>ir_foot_sensor</name>
   <version>0.0.0</version>
-  <description>The ir_feet_sensor package</description>
+  <description>The ir_foot_sensor package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
@@ -19,7 +19,7 @@
   <!-- Url tags are optional, but multiple are allowed, one per tag -->
   <!-- Optional attribute type can be: website, bugtracker, or repository -->
   <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/ir_feet_sensor</url> -->
+  <!-- <url type="website">http://wiki.ros.org/ir_foot_sensor</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->
@@ -41,7 +41,9 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>iri_base_driver</build_depend>
+  <build_depend>humanoid_common_msgs</build_depend>
   <run_depend>iri_base_driver</run_depend>
+  <run_depend>humanoid_common_msgs</run_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

ir_feet_sensor/src/ir_feet_sensor_driver.cpp → ir_feet_sensor/src/ir_foot_sensor_driver.cpp

-#include "ir_feet_sensor_driver.h"
+#include "ir_foot_sensor_driver.h"
 
-IrFeetSensorDriver::IrFeetSensorDriver(void)
+IrFootSensorDriver::IrFootSensorDriver(void)
 {
   //setDriverId(driver string id);
   this->device=NULL;
 }
 
-bool IrFeetSensorDriver::openDriver(void)
+bool IrFootSensorDriver::openDriver(void)
 {
+  std::stringstream name;
+
   try{
     if(this->device!=NULL)
     {
       delete this->device;
       this->device=NULL;
     }
-    this->device=new CIRFeet<CDynamixelServerSerial>("feet",this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
+    name << foot << "_" << this->config_.ir_foot_id;
+    this->device=new CIRFoot<CDynamixelServerSerial>(name.str(),this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
 
     return true;
   }catch(CException &e){
@@ -23,13 +26,13 @@ bool IrFeetSensorDriver::openDriver(void)
   }
 }
 
-bool IrFeetSensorDriver::closeDriver(void)
+bool IrFootSensorDriver::closeDriver(void)
 {
   try{
-    if(this->darwin!=NULL)
+    if(this->device!=NULL)
     {
-      delete this->darwin;
-      this->darwin=NULL;
+      delete this->device;
+      this->device=NULL;
     }
     return true;
   }catch(CException &e){
@@ -38,17 +41,17 @@ bool IrFeetSensorDriver::closeDriver(void)
   }
 }
 
-bool IrFeetSensorDriver::startDriver(void)
+bool IrFootSensorDriver::startDriver(void)
 {
   return true;
 }
 
-bool IrFeetSensorDriver::stopDriver(void)
+bool IrFootSensorDriver::stopDriver(void)
 {
   return true;
 }
 
-void IrFeetSensorDriver::config_update(Config& new_cfg, uint32_t level)
+void IrFootSensorDriver::config_update(Config& new_cfg, uint32_t level)
 {
   this->lock();
   
@@ -56,13 +59,13 @@ void IrFeetSensorDriver::config_update(Config& new_cfg, uint32_t level)
   // update driver with new_cfg data
   switch(this->getState())
   {
-    case IrFeetSensorDriver::CLOSED:
+    case IrFootSensorDriver::CLOSED:
       break;
 
-    case IrFeetSensorDriver::OPENED:
+    case IrFootSensorDriver::OPENED:
       break;
 
-    case IrFeetSensorDriver::RUNNING:
+    case IrFootSensorDriver::RUNNING:
       break;
   }
 
@@ -72,27 +75,37 @@ void IrFeetSensorDriver::config_update(Config& new_cfg, uint32_t level)
   this->unlock();
 }
 
-std::vector<double> IrFeetSensorDriver::get_all_sensor_voltages(void)
+std::vector<double> IrFootSensorDriver::get_all_sensor_voltages(void)
 {
-
+  if(this->device!=NULL)
+    return this->device->get_all_sensor_voltages();
+  else
+    return std::vector<double>();
 }
 
-std::vector<double> IrFeetSensorDriver::get_all_sensor_thresholds(void)
+std::vector<double> IrFootSensorDriver::get_all_sensor_thresholds(void)
 {
-
+  if(this->device!=NULL)
+    return this->device->get_all_sensor_thresholds();
+  else
+    return std::vector<double>();
 }
 
-void IrFeetSensorDriver::set_sensor_threshold(adc_dma_ch_t channel_id, double threshold)
+void IrFootSensorDriver::set_sensor_threshold(adc_dma_ch_t channel_id, double threshold)
 {
-
+  if(this->device!=NULL)
+    return this->device->set_sensor_threshold(channel_id, threshold);
 }
 
-std::vector<bool> IrFeetSensorDriver::get_all_sensor_status(void)
+std::vector<bool> IrFootSensorDriver::get_all_sensor_status(void)
 {
-
+  if(this->device!=NULL)
+    return this->device->get_all_sensor_status();
+  else
+    return std::vector<bool>();
 }
 
-IrFeetSensorDriver::~IrFeetSensorDriver(void)
+IrFootSensorDriver::~IrFootSensorDriver(void)
 {
   if(this->device!=NULL)
     delete this->device;

ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp → ir_feet_sensor/src/ir_foot_sensor_driver_node.cpp

-#include "ir_feet_sensor_driver_node.h"
+#include "ir_foot_sensor_driver_node.h"
 
-IrFeetSensorDriverNode::IrFeetSensorDriverNode(ros::NodeHandle &nh) : 
-  iri_base_driver::IriBaseNodeDriver<IrFeetSensorDriver>(nh)
+IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) : 
+  iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>(nh)
 {
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
 
   // [init publishers]
+  this->sensor_data_publisher_ = this->public_node_handle_.advertise<humanoid_common_msgs::ir_foot_data>("sensor_data", 1);
   
   // [init subscribers]
   
@@ -19,7 +20,7 @@ IrFeetSensorDriverNode::IrFeetSensorDriverNode(ros::NodeHandle &nh) :
   // [init action clients]
 }
 
-void IrFeetSensorDriverNode::mainNodeThread(void)
+void IrFootSensorDriverNode::mainNodeThread(void)
 {
   //lock access to driver if necessary
   this->driver_.lock();
@@ -27,12 +28,18 @@ void IrFeetSensorDriverNode::mainNodeThread(void)
   // [fill msg Header if necessary]
 
   // [fill msg structures]
+  // Initialize the topic message structure
+  //this->sensor_data_ir_foot_data_msg_.data = my_var;
+
   
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
+  // Uncomment the following line to publish the topic message
+  //this->sensor_data_publisher_.publish(this->sensor_data_ir_foot_data_msg_);
+
 
   //unlock access to driver if previously blocked
   this->driver_.unlock();
@@ -46,31 +53,31 @@ void IrFeetSensorDriverNode::mainNodeThread(void)
 
 /*  [action requests] */
 
-void IrFeetSensorDriverNode::postNodeOpenHook(void)
+void IrFootSensorDriverNode::postNodeOpenHook(void)
 {
 }
 
-void IrFeetSensorDriverNode::addNodeDiagnostics(void)
+void IrFootSensorDriverNode::addNodeDiagnostics(void)
 {
 }
 
-void IrFeetSensorDriverNode::addNodeOpenedTests(void)
+void IrFootSensorDriverNode::addNodeOpenedTests(void)
 {
 }
 
-void IrFeetSensorDriverNode::addNodeStoppedTests(void)
+void IrFootSensorDriverNode::addNodeStoppedTests(void)
 {
 }
 
-void IrFeetSensorDriverNode::addNodeRunningTests(void)
+void IrFootSensorDriverNode::addNodeRunningTests(void)
 {
 }
 
-void IrFeetSensorDriverNode::reconfigureNodeHook(int level)
+void IrFootSensorDriverNode::reconfigureNodeHook(int level)
 {
 }
 
-IrFeetSensorDriverNode::~IrFeetSensorDriverNode(void)
+IrFootSensorDriverNode::~IrFootSensorDriverNode(void)
 {
   // [free dynamic memory]
 }
@@ -78,5 +85,5 @@ IrFeetSensorDriverNode::~IrFeetSensorDriverNode(void)
 /* main function */
 int main(int argc,char *argv[])
 {
-  return driver_base::main<IrFeetSensorDriverNode>(argc, argv, "ir_feet_sensor_driver_node");
+  return driver_base::main<IrFootSensorDriverNode>(argc, argv, "ir_foot_sensor_driver_node");
 }