Added a dynamixel servo class to implement the servo functionality.

include/dynamixel_servo.h

+#ifndef DYNAMIXEL_SERVO_H
+#define DYNAMIXEL_SERVO_H
+
+#include <hardware_interface/joint_command_interface.h>
+#include <control_toolbox/pid.h>
+#include <boost/shared_ptr.hpp>
+
+#include "dynamixel_slave_serial.h"
+
+namespace dynamixel_robot_gazebo
+{
+  class CDynServo
+  { 
+    private:
+      typedef typename hardware_interface::EffortJointInterface::ResourceHandleType JointHandle;
+      JointHandle joint;
+      typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
+      PidPtr pid;
+      unsigned char id;
+    protected:
+      void init_pid(const std::string &joint_name,ros::NodeHandle &controller_nh);
+      void on_ping(void);
+      unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data);
+      unsigned char on_write(unsigned short int address, unsigned short int length, unsigned char *data);
+   public:
+      CDynServo(const std::string &joint_name,CDynamixelSlaveSerial *dyn_slave,hardware_interface::EffortJointInterface* hw,ros::NodeHandle &controller_nh);
+      void update(const ros::Duration& period);
+      unsigned char get_id(void);
+      ~CDynServo();
+  };
+}
+
+#endif

src/dynamixel_servo.cpp

+#include "dynamixel_slave_serial.h"
+#include "dynamixel_servo.h"
+
+namespace dynamixel_robot_gazebo
+{
+  CDynServo::CDynServo(const std::string &joint_name,CDynamixelSlaveSerial *dyn_slave,hardware_interface::EffortJointInterface* hw,ros::NodeHandle &controller_nh)
+  {
+    std::string model;
+    int id;
+
+    // Joint handle
+    try {
+      this->joint=hw->getHandle(joint_name);
+    }catch(...){
+      ROS_ERROR_STREAM("Dynamixel servo: Could not find joint '" << joint_name);
+      throw;
+    }
+    // Node handle to PID gains
+    ros::NodeHandle servo_nh(controller_nh,joint_name);
+    servo_nh.getParam("model",model);
+    if(model=="AX12W")
+    {
+    }
+    else if(model=="AX12A")
+    {
+    }
+    else if(model=="AX18A")
+    {
+    }
+    else if(model=="MX12W")
+    {
+    }
+    else if(model=="MX28v1")
+    {
+      this->init_pid(joint_name,controller_nh);
+    }
+    else if(model=="MX64v1")
+    {
+      this->init_pid(joint_name,controller_nh);
+    }
+    else if(model=="MX106v1")
+    {
+      this->init_pid(joint_name,controller_nh);
+    }
+    else if(model=="MX28v2")
+    {
+      this->init_pid(joint_name,controller_nh);
+    }
+    else if(model=="MX64v2")
+    {
+      this->init_pid(joint_name,controller_nh);
+    }
+    else if(model=="MX106v2")
+    {
+      this->init_pid(joint_name,controller_nh);
+    }
+    else if(model=="XL320")
+    {
+      this->init_pid(joint_name,controller_nh);
+    }
+    else if(model=="XL430")
+    {
+      this->init_pid(joint_name,controller_nh);
+    }
+    else
+    {
+      ROS_ERROR_STREAM("Dynamixel servo: Invalid servo model");
+      throw;
+    }
+
+    // Init PID gains from ROS parameter server
+    servo_nh.getParam("id",id);
+    dyn_slave->add_slave(id,boost::bind(&CDynServo::on_ping,this),boost::bind(&CDynServo::on_read,this,_1,_2,_3),boost::bind(&CDynServo::on_write,this,_1,_2,_3));
+    this->id=id;
+  }
+
+  void CDynServo::init_pid(const std::string &joint_name,ros::NodeHandle &controller_nh)
+  {
+    ros::NodeHandle gain_nh(controller_nh,joint_name+std::string("/gains"));
+    this->pid.reset(new control_toolbox::Pid());
+    if(!this->pid->init(gain_nh))
+    {
+      ROS_WARN_STREAM("Dynamixel gazebo plugin: Failed to initialize PID gains from ROS parameter server.");
+      throw;
+    } 
+  }
+
+  void CDynServo::on_ping(void)
+  {
+
+  }
+
+  unsigned char CDynServo::on_read(unsigned short int address, unsigned short int length, unsigned char *data)
+  {
+
+  }
+
+  unsigned char CDynServo::on_write(unsigned short int address, unsigned short int length, unsigned char *data)
+  {
+
+  }
+
+  void CDynServo::update(const ros::Duration& period)
+  {
+    double real_angle,target_angle,command;
+
+    real_angle=this->joint.getPosition();
+    target_angle=0.0;//((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0;
+    command=this->pid->computeCommand(target_angle-real_angle,period);
+    this->joint.setCommand(command);
+  }
+
+  unsigned char CDynServo::get_id(void)
+  {
+    return this->id;
+  }
+
+  CDynServo::~CDynServo()
+  {
+
+  }
+}