Added attributes and functions in the driver to be used in the nodelet version...

Added attributes and functions in the driver to be used in the nodelet version (the parameters for all the nodes are the same).
Changed the NodeHandle used to create topics in different namespaces.
Changed the parameters of the launch files.

ir_foot_sensor/include/ir_foot_sensor_driver.h

@@ -56,6 +56,9 @@ class IrFootSensorDriver : public iri_base_driver::IriBaseDriver
   private:
     // private attributes and methods
     CIRFeet<CDynamixelServerSerial> *device;
+    std::string dyn_device;
+    unsigned int dyn_baudrate;
+    unsigned char dev_id;
   public:
    /**
     * \brief define config type
@@ -150,6 +153,9 @@ class IrFootSensorDriver : public iri_base_driver::IriBaseDriver
     std::vector<double> get_all_sensor_thresholds(void);
     void set_sensor_threshold(adc_dma_ch_t channel_id, double threshold);
     std::vector<bool> get_all_sensor_status(void);
+    void set_dynamixel_device(std::string &device);
+    void set_dynamixel_baudrate(unsigned int baudrate);
+    void set_device_id(unsigned char dev_id);
 
    /**
     * \brief Destructor

ir_foot_sensor/include/ir_foot_sensor_driver_node.h

@@ -187,6 +187,7 @@ class IrFootSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFootS
 class IrFootSensorDriverNodelet : public nodelet::Nodelet
 {
   private:
+    ros::NodeHandle nodelet_nh;
     IrFootSensorDriverNode *node;
     virtual void onInit();// initialization function
     // thread attributes

ir_foot_sensor/launch/darwin_left_foot.launch

@@ -4,8 +4,8 @@
         pkg="ir_foot_sensor" 
         type="ir_foot_sensor" 
         output="screen">
-    <param name="dyn_serial" value="A4008atn"/>
-    <param name="dyn_baudrate" value="1000000"/>
+    <param name="dyn_serial" value="/dev/ttyUSB0"/>
+    <param name="dyn_baudrate" value="115200"/>
     <param name="ir_foot_id" value="1"/>
     <remap from="/darwin_left_foot/sensor_data"
              to="/darwin/sensors/left_foot_data"/>

ir_foot_sensor/launch/darwin_right_foot.launch

@@ -4,8 +4,8 @@
         pkg="ir_foot_sensor" 
         type="ir_foot_sensor" 
         output="screen">
-    <param name="dyn_serial" value="A4008atn"/>
-    <param name="dyn_baudrate" value="1000000"/>
+    <param name="dyn_serial" value="/dev/ttyUSB0"/>
+    <param name="dyn_baudrate" value="115200"/>
     <param name="ir_foot_id" value="2"/>
     <remap from="/darwin_right_foot/sensor_data"
              to="/darwin/sensors/right_foot_data"/>

ir_foot_sensor/launch/darwin_two_feet_nodelet.launch

@@ -13,8 +13,8 @@
         name="darwin_left_foot" 
         args="load ir_foot_sensor/IrFootSensorDriverNodelet dynamixel_devices"
         output="screen">
-    <param name="dyn_serial" value="A4008atn"/>
-    <param name="dyn_baudrate" value="1000000"/>
+    <param name="dyn_serial" value="/dev/ttyUSB0"/>
+    <param name="dyn_baudrate" value="115200"/>
     <param name="ir_foot_id" value="1"/>
     <remap from="/darwin_left_foot/sensor_data"
              to="/darwin/sensors/left_foot_data"/>
@@ -26,8 +26,8 @@
         name="darwin_right_foot" 
         args="load ir_foot_sensor/IrFootSensorDriverNodelet dynamixel_devices"
         output="screen">
-    <param name="dyn_serial" value="A4008atn"/>
-    <param name="dyn_baudrate" value="1000000"/>
+    <param name="dyn_serial" value="/dev/ttyUSB0"/>
+    <param name="dyn_baudrate" value="115200"/>
     <param name="ir_foot_id" value="2"/>
     <remap from="/darwin_right_foot/sensor_data"
              to="/darwin/sensors/right_foot_data"/>

ir_foot_sensor/src/ir_foot_sensor_driver.cpp

@@ -4,6 +4,9 @@ IrFootSensorDriver::IrFootSensorDriver(void)
 {
   //setDriverId(driver string id);
   this->device=NULL;
+  this->dyn_device="";
+  this->dyn_baudrate=-1;
+  this->dev_id=-1;
 }
 
 bool IrFootSensorDriver::openDriver(void)
@@ -17,7 +20,10 @@ bool IrFootSensorDriver::openDriver(void)
       this->device=NULL;
     }
     name << "foot_" << this->config_.ir_foot_id;
-    this->device=new CIRFeet<CDynamixelServerSerial>(name.str(),this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
+    if(this->dyn_device!="" && this->dyn_baudrate!=-1 && this->dev_id!=-1)
+      this->device=new CIRFeet<CDynamixelServerSerial>(name.str(),this->dyn_device,this->dyn_baudrate,this->dev_id);
+    else
+      this->device=new CIRFeet<CDynamixelServerSerial>(name.str(),this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
 
     return true;
   }catch(CException &e){
@@ -105,6 +111,21 @@ std::vector<bool> IrFootSensorDriver::get_all_sensor_status(void)
     return std::vector<bool>();
 }
 
+void IrFootSensorDriver::set_dynamixel_device(std::string &device)
+{
+  this->dyn_device=device;
+}
+
+void IrFootSensorDriver::set_dynamixel_baudrate(unsigned int baudrate)
+{
+  this->dyn_baudrate=baudrate;
+}
+
+void IrFootSensorDriver::set_device_id(unsigned char dev_id)
+{
+  this->dev_id=dev_id;
+}
+
 IrFootSensorDriver::~IrFootSensorDriver(void)
 {
   if(this->device!=NULL)

ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp

@@ -3,11 +3,13 @@
 IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) : 
   iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>(nh)
 {
+  std::string dyn_serial="";
+  int dyn_baudrate=-1,dev_id=-1;
   //init class attributes if necessary
   this->loop_rate_ = 10;//in [Hz]
 
   // [init publishers]
-  this->sensor_data_publisher_ = this->public_node_handle_.advertise<humanoid_common_msgs::ir_foot_data>("sensor_data", 1);
+  this->sensor_data_publisher_ = this->node_handle_.advertise<humanoid_common_msgs::ir_foot_data>("sensor_data", 1);
   
   // [init subscribers]
   
@@ -29,6 +31,13 @@ IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) :
   this->sensor_data_ir_foot_data_msg_.names[7]="front_left";
   this->sensor_data_ir_foot_data_msg_.names[8]="front_right";
   this->sensor_data_ir_foot_data_msg_.names[9]="front_analog";
+
+  this->node_handle_.getParam("dyn_serial",dyn_serial);
+  this->driver_.set_dynamixel_device(dyn_serial);
+  this->node_handle_.getParam("dyn_baudrate",dyn_baudrate);
+  this->driver_.set_dynamixel_baudrate(dyn_baudrate);
+  this->node_handle_.getParam("ir_foot_id",dev_id);
+  this->driver_.set_device_id(dev_id);
 }
 
 void IrFootSensorDriverNode::mainNodeThread(void)
@@ -111,7 +120,7 @@ int main(int argc,char *argv[])
 
 #include <pluginlib/class_list_macros.h>
 
-IrFootSensorDriverNodelet::IrFootSensorDriverNodelet()
+IrFootSensorDriverNodelet::IrFootSensorDriverNodelet() 
 {
   this->node=NULL;
 }
@@ -131,7 +140,8 @@ IrFootSensorDriverNodelet::~IrFootSensorDriverNodelet(void)
 void IrFootSensorDriverNodelet::onInit()
 {
   // initialize the driver node
-  this->node=new IrFootSensorDriverNode(getPrivateNodeHandle());
+  this->nodelet_nh=ros::NodeHandle(getPrivateNodeHandle(),getName());
+  this->node=new IrFootSensorDriverNode(this->nodelet_nh);
   // initialize the thread
   this->thread_server=CThreadServer::instance();
   this->spin_thread_id=getName() + "_firewire_nodelet_spin";