Skip to content
Snippets Groups Projects
darwin_controller_impl.h 37.90 KiB
#ifndef DARWIN_CONTROLLER_IMP_H
#define DARWIN_CONTROLLER_IMP_H

#include "stm32f1xx.h"
#include "motion_manager.h"
#include "action.h"
#include "action_id.h"
#include "walking.h"
#include "joint_motion.h"
#include "head_tracking.h"
#include "ram.h"
#include "imu.h"
#include "darwin_math.h"
#ifdef _HAVE_XSD
#include "../src/xml/darwin_config.hxx"
#endif

extern "C" {
  // motion_manager private function declaration
  void TIM5_IRQHandler(void);
}

// motion manager external variables
extern int num_servos;
extern double real_angles[PAGE_MAX_NUM_SERVOS];

// action module external variables
extern TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];

//IMU module external variables
extern int32_t gyro_data[3];
extern int32_t accel_data[3];

//parameters default values
#define       DEFAULT_ENABLE_SMART_CHARGER                false
#define       DEFAULT_INITIAL_CHARGE                      2200.0
#define       DEFAULT_DISCHARGE_RATE                      6000.0
#define       DEFAULT_CHARGE_CURRENT                      2200.0
#define       DEFAULT_CHARGE_STATION_LINK                 "none"
#define       DEFAULT_CHARGE_ROBOT_LINK                   "none"

namespace darwin_controller
{
  namespace internal
  {
    std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string& param_name)
    {
      using namespace XmlRpc;
      XmlRpcValue xml_array;
      if (!nh.getParam(param_name, xml_array))
      {
	ROS_ERROR_STREAM("CDarwinSim : Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
	return std::vector<std::string>();
      }
      if (xml_array.getType() != XmlRpcValue::TypeArray)
      {
	ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter is not an array (namespace: " <<
	    nh.getNamespace() << ").");
	return std::vector<std::string>();
      }

      std::vector<std::string> out;
      for (int i = 0; i < xml_array.size(); ++i)
      {
	if (xml_array[i].getType() != XmlRpcValue::TypeString)
	{
	  ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter contains a non-string element (namespace: " <<
	      nh.getNamespace() << ").");
	  return std::vector<std::string>();
	}
	out.push_back(static_cast<std::string>(xml_array[i]));
      }
      return out;
    }

    boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
    {
      boost::shared_ptr<urdf::Model> urdf(new urdf::Model);

      std::string urdf_str;
      // Check for robot_description in proper namespace
      if (nh.getParam(param_name, urdf_str))
      {
	if (!urdf->initString(urdf_str))
	{
	  ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
	      nh.getNamespace() << ").");
	  return boost::shared_ptr<urdf::Model>();
	}
      }
      // Check for robot_description in root
      else if (!urdf->initParam("robot_description"))
      {
	ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter");
	return boost::shared_ptr<urdf::Model>();
      }
      return urdf;
    }

    typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
    std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
    {
      std::vector<UrdfJointConstPtr> out;
      for (unsigned int i = 0; i < joint_names.size(); ++i)
      {
	UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
	if (urdf_joint)
	{
	  out.push_back(urdf_joint);
	}
	else
	{
	  ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names[i] << "' in URDF model.");
	  return std::vector<UrdfJointConstPtr>();
	}
      }
      return out;
    }

    std::string getLeafNamespace(const ros::NodeHandle& nh)
    {
      const std::string complete_ns = nh.getNamespace();
      std::size_t id   = complete_ns.find_last_of("/");
      return complete_ns.substr(id + 1);
    }

  } // namespace

  template <class HardwareInterface>
    inline void DarwinController<HardwareInterface>::starting(const ros::Time& time)
    {
      for(unsigned int i = 0; i <this->joints_.size(); ++i)
      {
        pids_[i]->reset();
        this->joints_[i].setCommand(0.0);
      }
    }

  template <class HardwareInterface>
    inline void DarwinController<HardwareInterface>::stopping(const ros::Time& time)
    {
    }

  template <class HardwareInterface>
    DarwinController<HardwareInterface>::DarwinController()
    {
      this->motion_action_aserver_=NULL;
    }

  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::init(HardwareInterface* hw,ros::NodeHandle&   root_nh,ros::NodeHandle&   controller_nh)
    {
      using namespace internal;

      // Cache controller node handle
      controller_nh_ = controller_nh;

      // smart charger interface
      this->enable_smart_charger=DEFAULT_ENABLE_SMART_CHARGER;
      controller_nh_.getParam("enable_smart_charger", this->enable_smart_charger);
      ROS_DEBUG_STREAM_NAMED(name_, "Smart charger is: " << this->enable_smart_charger);
      if(this->enable_smart_charger)
      {
        // initialize gazebo interfaces
        this->gazebo_node=gazebo::transport::NodePtr(new gazebo::transport::Node());
        this->gazebo_node->Init();
        this->contacts_subs=this->gazebo_node->Subscribe("/gazebo/default/physics/contacts",&DarwinController<HardwareInterface>::OnContacts,this);
        // initialize publisher
        this->smart_charger_data_publisher_ = root_nh.advertise<humanoid_common_msgs::smart_charger_data>("robot/smart_charger_data", 1);
        // initialize service servers
        this->get_smart_charger_config_server_=root_nh.advertiseService("robot/get_smart_charger_config", &DarwinController<HardwareInterface>::get_smart_charger_configCallback, this);
        this->set_smart_charger_config_server_=root_nh.advertiseService("robot/set_smart_charger_config", &DarwinController<HardwareInterface>::set_smart_charger_configCallback, this);
        // read parameters
        this->initial_charge=DEFAULT_INITIAL_CHARGE;
        controller_nh_.getParam("initial_charge", this->initial_charge);
        ROS_DEBUG_STREAM_NAMED(name_, "Initial charge set to: " << this->initial_charge);
        this->current_charge=this->initial_charge;
        this->discharge_rate=DEFAULT_DISCHARGE_RATE;
        controller_nh_.getParam("discharge_rate", this->discharge_rate);
        ROS_DEBUG_STREAM_NAMED(name_, "Discharge rate set to: " << this->discharge_rate);
        this->charge_current=DEFAULT_CHARGE_CURRENT;
        controller_nh_.getParam("charge_current", this->charge_current);
        ROS_DEBUG_STREAM_NAMED(name_, "Charge current set to: " << this->charge_current);
        this->charge_station_link=DEFAULT_CHARGE_STATION_LINK;
        controller_nh_.getParam("charge_station_link", this->charge_station_link);
        ROS_DEBUG_STREAM_NAMED(name_, "Charger station link set to: " << this->charge_station_link);
        this->charge_robot_link=DEFAULT_CHARGE_ROBOT_LINK;
        controller_nh_.getParam("charge_robot_link", this->charge_robot_link);
        ROS_DEBUG_STREAM_NAMED(name_, "Charger robot link set to: " << this->charge_robot_link);
        
        this->charger_connected=false;
      }

      /* initialize actions */
      this->motion_action_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_motionAction>(root_nh,"robot/motion_action");
      this->motion_action_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_startCallback, this, _1));
      this->motion_action_aserver_->registerStopCallback(boost::bind(& DarwinController<HardwareInterface>::motion_action_stopCallback, this));
      this->motion_action_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_isFinishedCallback, this));
      this->motion_action_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_hasSucceedCallback, this));
      this->motion_action_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_getResultCallback, this, _1));
      this->motion_action_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_getFeedbackCallback, this, _1));
      this->motion_action_aserver_->start();

      this->joint_trajectory_aserver_=new IriActionServer<control_msgs::JointTrajectoryAction>(root_nh,"robot/joint_trajectory");
      this->joint_trajectory_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_startCallback, this, _1));
      this->joint_trajectory_aserver_->registerStopCallback(boost::bind(& DarwinController<HardwareInterface>::joint_trajectory_stopCallback, this));
      this->joint_trajectory_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_isFinishedCallback, this));
      this->joint_trajectory_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_hasSucceedCallback, this));
      this->joint_trajectory_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_getResultCallback, this, _1));
      this->joint_trajectory_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::joint_trajectory_getFeedbackCallback, this, _1));
      this->joint_trajectory_aserver_->start();

      this->follow_target_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_follow_targetAction>(root_nh,"robot/head_follow_target");
      this->follow_target_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_startCallback, this, _1));
      this->follow_target_aserver_->registerStopCallback(boost::bind(& DarwinController<HardwareInterface>::follow_target_stopCallback, this));
      this->follow_target_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_isFinishedCallback, this));
      this->follow_target_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_hasSucceedCallback, this));
      this->follow_target_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_getResultCallback, this, _1));
      this->follow_target_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::follow_target_getFeedbackCallback, this, _1));
      this->follow_target_aserver_->start();

      /* initialize services */
      this->set_servo_modules_server_=root_nh.advertiseService("robot/set_servo_modules", &DarwinController<HardwareInterface>::set_servo_modulesCallback, this);
      this->set_walk_params_server_=root_nh.advertiseService("robot/set_walk_params", &DarwinController<HardwareInterface>::set_walk_paramsCallback, this);
      this->get_walk_params_server_=root_nh.advertiseService("robot/get_walk_params", &DarwinController<HardwareInterface>::get_walk_paramsCallback, this);
      this->set_pan_pid_server_=root_nh.advertiseService("robot/set_pan_pid", &DarwinController<HardwareInterface>::set_pan_pidCallback, this);
      this->get_pan_pid_server_=root_nh.advertiseService("robot/get_pan_pid", &DarwinController<HardwareInterface>::get_pan_pidCallback, this);
      this->set_tilt_pid_server_=root_nh.advertiseService("robot/set_tilt_pid", &DarwinController<HardwareInterface>::set_tilt_pidCallback, this);
      this->get_tilt_pid_server_=root_nh.advertiseService("robot/get_tilt_pid", &DarwinController<HardwareInterface>::get_tilt_pidCallback, this);

      // initialize topics
      this->gyro_sub=root_nh.subscribe("sensors/raw_gyro", 1, &DarwinController<HardwareInterface>::gyro_callback,this);
      this->accel_sub=root_nh.subscribe("sensors/raw_accel", 1, &DarwinController<HardwareInterface>::accel_callback,this);
      this->cmd_vel_sub=root_nh.subscribe("robot/cmd_vel", 1, &DarwinController<HardwareInterface>::cmd_vel_callback,this);
      this->head_target_sub=root_nh.subscribe("robot/head_target", 1, &DarwinController<HardwareInterface>::head_target_callback,this);

      // initialize publisher
      this->fallen_state_publisher_ = root_nh.advertise<std_msgs::Int8>("robot/fallen_state", 1);

      // Controller name
      name_=getLeafNamespace(controller_nh_);

      // List of controlled joints
      joint_names_=getStrings(controller_nh_,"joints");
      if(joint_names_.empty()) 
        return false;
      const unsigned int n_joints=joint_names_.size();
 
      // URDF joints
      boost::shared_ptr<urdf::Model> urdf=getUrdf(root_nh, "robot_description");
      if(!urdf) 
        return false;

      std::vector<UrdfJointConstPtr> urdf_joints=getUrdfJoints(*urdf, joint_names_);
      if(urdf_joints.empty()) 
        return false;
      assert(n_joints==urdf_joints.size());
      num_servos=n_joints;// set the number of servos to the number of joint of the urdf model

      // Initialize members
      this->joints_.resize(n_joints);
      this->pids_.resize(n_joints);
      for(unsigned int i=0;i<n_joints;++i)
      {
	// Joint handle
	try {
          joints_[i]=hw->getHandle(joint_names_[i]);
        }catch(...){
	  ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names_[i] << "' in '" <<
	      this->getHardwareInterfaceType() << "'.");
	  return false;
	}
        // Node handle to PID gains
        ros::NodeHandle joint_nh(this->controller_nh_,std::string("gains/")+joint_names_[i]);

        // Init PID gains from ROS parameter server
        this->pids_[i].reset(new control_toolbox::Pid());
        if(!this->pids_[i]->init(joint_nh))
        {
          ROS_WARN_STREAM("CDarwinSim : Failed to initialize PID gains from ROS parameter server.");
          return false;
        }

	// Whether a joint is continuous (ie. has angle wraparound)
	const std::string not_if = urdf_joints[i]->type == urdf::Joint::CONTINUOUS ? "" : "non-";

	ROS_DEBUG_STREAM("CDarwinSim : Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << this->getHardwareInterfaceType() << "'.");
      }

      ROS_DEBUG_STREAM("CDarwinSim : Initialized controller '" << name_ << "' with:" <<
	  "\n- Number of joints: " << joints_.size() <<
	  "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'");

      /* initialize the RAM */
      ram_init();
      /* initialize the IMU */
      imu_init();
      /* initialize motion modules */
      manager_init(7800);// motion manager period in us
      ROS_INFO("CDarwinSim : motion manager initialized");
      /* go to the walk ready position */
      for(unsigned int i=0;i<n_joints;++i)
      {
        manager_set_module(i+1,MM_ACTION);
        manager_enable_servo(i+1);
      }
      manager_enable_balance();// enable balance
      action_load_page(WALK_READY);
      action_start_page();

      return true;
    }

  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
    {
      static bool gyro_calibrated=false;
      static ros::Time last_time=ros::Time(0,0);
      ros::Time current_time=ros::Time::now();
      std::vector<double> target_angles(joints_.size());

      /* get the actual simulation angles */
      for(unsigned int i=0;i<this->joints_.size();++i)
        real_angles[i]=joints_[i].getPosition();
        __HAL_TIM_SET_FLAG(TIM5,TIM_FLAG_CC1);
        __HAL_TIM_SET_IT_SOURCE(TIM5,TIM_IT_CC1);
        TIM5_IRQHandler();
      for (unsigned int i = 0; i < this->joints_.size(); ++i)
      {
        target_angles[i] = ((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0;
        const double command = pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
        this->joints_[i].setCommand(command);
      }
      if(is_walking() && ((current_time-this->walk_timeout).toSec()>1.0))
        walking_stop();
      if(this->enable_smart_charger)
      {
        this->current_charge-=this->discharge_rate*period.toSec()/3600.0;
        this->smart_charger_data_smart_charger_data_msg_.header.stamp=ros::Time::now();
        if(this->charger_connected)
        {
          this->smart_charger_data_smart_charger_data_msg_.avg_time_empty=(uint16_t)-1;
          this->smart_charger_data_smart_charger_data_msg_.avg_time_full=(uint16_t)(((this->initial_charge-this->current_charge)/this->charge_current)*60.0);
          this->smart_charger_data_smart_charger_data_msg_.batt_status="Connected and charging";
        }
        else
        {
          this->smart_charger_data_smart_charger_data_msg_.avg_time_empty=(uint16_t)((this->current_charge/this->discharge_rate)*60.0);
          this->smart_charger_data_smart_charger_data_msg_.avg_time_full=(uint16_t)-1;
        }
        this->smart_charger_data_publisher_.publish(this->smart_charger_data_smart_charger_data_msg_);
      }
      // detect fall condition
      this->fallen_state_msg_.data=(unsigned char)manager_get_fallen_position();
      this->fallen_state_publisher_.publish(this->fallen_state_msg_);
    }

  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::OnContacts(ConstContactsPtr &msg)
    {
      for(unsigned int i=0;i<msg->contact_size();i++)
      {
        if(msg->contact(i).collision1()==this->charge_station_link)
        {
          if(msg->contact(i).collision2()==this->charge_robot_link)
          {
            this->charger_connected=true;
            break;
          }
          else
            this->charger_connected=false;
        }
        else if(msg->contact(i).collision1()==this->charge_robot_link)
        {
          if(msg->contact(i).collision2()==this->charge_station_link)
          {
            this->charger_connected=true;
            break;
          }
          else
            this->charger_connected=false;
        }
        else
          this->charger_connected=false;
      }
      if(this->charger_connected)
        ROS_INFO("Charger connected");
    }

/************************************ motion_action *********************************************/
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal)
    {
      ROS_INFO_STREAM("CDarwinSim : Start motion action " << goal->motion_id);
      /* load a page and start execution */
      if(!action_load_page(goal->motion_id))
        ROS_WARN("CDarwinSim : Impossible to load the desired page");
      else
      {
        ROS_INFO("CDarwinSim : Page loaded successfully");
        action_start_page();
      }
    }
     
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::motion_action_stopCallback(void)
    {
      ROS_INFO("CDarwinSim : Stop motion action");
      action_stop_page();
    }
     
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::motion_action_isFinishedCallback(void)
    {
      if(action_is_running())
        return false;
      else
        return true;
    }
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::motion_action_hasSucceedCallback(void)
    {
      return true;
    }
     
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::motion_action_getResultCallback(humanoid_common_msgs::humanoid_motionResultPtr& result)
    {
  
    }
  
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::motion_action_getFeedbackCallback(humanoid_common_msgs::humanoid_motionFeedbackPtr& feedback)
    {
      feedback->current_page=action_get_current_page();
      feedback->current_step=action_get_current_step();
    }
/************************************ motion_action *********************************************/

/************************************ joint_trajectory_action *********************************************/
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::joint_trajectory_startCallback(const control_msgs::JointTrajectoryGoalConstPtr& goal)
    {
      unsigned int i=0,j=0;
      uint16_t angle;
      uint8_t speed,accel;
      uint32_t servos=0;

      ROS_INFO("CDarwinSim : Start joint trajectory action");
      if(goal->trajectory.points.size()==1)
      {
        for(i=0;i<goal->trajectory.joint_names.size();i++)
        {
          for(j=0;j<MANAGER_MAX_NUM_SERVOS;j++)
          {
            if(servo_names[j]==goal->trajectory.joint_names[i])
            {
              angle=(goal->trajectory.points[0].positions[i]*180.0/3.14159)*128.0;
              ram_write_word(DARWIN_JOINT_SERVO0_ANGLE_L+j*2,angle);
              speed=(goal->trajectory.points[0].velocities[i]*180.0/3.14159);
              ram_write_byte(DARWIN_JOINT_SERVO0_SPEED+j,speed);
              accel=(goal->trajectory.points[0].accelerations[i]*180.0/3.14159);
              ram_write_byte(DARWIN_JOINT_SERVO0_ACCEL+j,accel);
              servos|=0x00000001<<j;
            }
          }
        }
        joint_motion_set_group_servos(0,servos);
        joint_motion_start(0);
      }
    }

  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::joint_trajectory_stopCallback(void)
    {
      ROS_INFO("CDarwinSim : Stop joint trajectory action");
      joint_motion_stop(0);
    }

  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::joint_trajectory_isFinishedCallback(void)
    {
      if(are_joints_moving(0))
        return false;
      else
        return true;
    }

  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::joint_trajectory_hasSucceedCallback(void)
    {
      return true;
    }

  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::joint_trajectory_getResultCallback(control_msgs::JointTrajectoryResultPtr& result)
    {

    }

  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::joint_trajectory_getFeedbackCallback(control_msgs::JointTrajectoryFeedbackPtr& feedback)
    {
    }
/************************************ joint_trajectory_action *********************************************/

/************************************ follow_target_action *********************************************/
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::follow_target_startCallback(const humanoid_common_msgs::humanoid_follow_targetGoalConstPtr& goal)
    {
      ROS_INFO("CDarwinSim : Start head tracking action");
      if(goal->pan_range.size()==2)
        head_tracking_set_pan_range(((goal->pan_range[0]*180.0)/3.14159)*128.0,((goal->pan_range[1]*180.0)/3.14159)*128.0);
      if(goal->tilt_range.size()==2)
        head_tracking_set_tilt_range(((goal->tilt_range[0]*180.0)/3.14159)*128.0,((goal->tilt_range[1]*180.0)/3.14159)*128.0);
      head_tracking_set_target_pan(((goal->target_pan*180.0)/3.14159)*128.0);
      head_tracking_set_target_tilt(((goal->target_tilt*180.0)/3.14159)*128.0);
      head_tracking_start();
    }

  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::follow_target_stopCallback(void)
    {
      ROS_INFO("CDarwinSim : Stop head_tracking action");
      head_tracking_stop();
    }

  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::follow_target_isFinishedCallback(void)
    {
      if(head_is_tracking())
        return false;
      else
        return true;
    }

  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::follow_target_hasSucceedCallback(void)
    {
      return true;
    }

  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::follow_target_getResultCallback(humanoid_common_msgs::humanoid_follow_targetResultPtr& result)
    {

    }

  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::follow_target_getFeedbackCallback(humanoid_common_msgs::humanoid_follow_targetFeedbackPtr& feedback)
    {
      feedback->current_pan=((manager_current_angles[L_PAN]*3.14159)/180.0)/65536.0;
      feedback->current_tilt=((manager_current_angles[L_TILT]*3.14159)/180.0)/65536.0;
    }
/************************************ follow_target_action *********************************************/


/************************************ set_walk_params *******************************************/
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res)
    {
      ROS_INFO("CDarwinSim : Setting walk parameters ...");
      this->walk_access.enter();
      ram_write_byte(DARWIN_WALK_SWING_RIGHT_LEFT,(unsigned char)(req.params.Y_SWAP_AMPLITUDE*1000.0));
      ram_write_byte(DARWIN_WALK_SWING_TOP_DOWN,(unsigned char)(req.params.Z_SWAP_AMPLITUDE*1000.0));
      ram_write_byte(DARWIN_WALK_ARM_SWING_GAIN,(unsigned char)(req.params.ARM_SWING_GAIN*32.0));
      ram_write_byte(DARWIN_WALK_PELVIS_OFFSET,(unsigned char)((req.params.PELVIS_OFFSET*180.0/PI)*8.0));
      ram_write_word(DARWIN_WALK_HIP_PITCH_OFF_L,(unsigned short int)((req.params.HIP_PITCH_OFFSET*180.0/PI)*1024.0));
      ram_write_byte(DARWIN_WALK_X_OFFSET,(unsigned char)(req.params.X_OFFSET*1000.0));
      ram_write_byte(DARWIN_WALK_Y_OFFSET,(unsigned char)(req.params.Y_OFFSET*1000.0));
      ram_write_byte(DARWIN_WALK_Z_OFFSET,(unsigned char)(req.params.Z_OFFSET*1000.0));
      ram_write_byte(DARWIN_WALK_YAW_OFFSET,(unsigned char)((req.params.A_OFFSET*180/PI)*8.0));
      ram_write_byte(DARWIN_WALK_PITCH_OFFSET,(unsigned char)((req.params.P_OFFSET*180/PI)*8.0));
      ram_write_byte(DARWIN_WALK_ROLL_OFFSET,(unsigned char)((req.params.R_OFFSET*180/PI)*8.0));
      ram_write_word(DARWIN_WALK_PERIOD_TIME_L,(unsigned short int)(req.params.PERIOD_TIME*1000.0));
      ram_write_byte(DARWIN_WALK_DSP_RATIO,(unsigned char)(req.params.DSP_RATIO*256.0));
      ram_write_byte(DARWIN_WALK_STEP_FW_BW_RATIO,(unsigned char)(req.params.STEP_FB_RATIO*256.0));
      ram_write_byte(DARWIN_WALK_FOOT_HEIGHT,(unsigned char)(req.params.FOOT_HEIGHT*1000.0));
      ram_write_byte(DARWIN_WALK_MAX_VEL,(unsigned char)(req.params.MAX_VEL*1000.0));
      ram_write_byte(DARWIN_WALK_MAX_ROT_VEL,(unsigned char)((req.params.MAX_ROT_VEL*1440.0)/PI));
      this->walk_access.exit();

      res.ret=true;
    }
/************************************ set_walk_params *******************************************/

/************************************ get_walk_params *******************************************/
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::get_walk_paramsCallback(humanoid_common_msgs::get_walk_params::Request &req, humanoid_common_msgs::get_walk_params::Response &res)
    {
      unsigned char byte_value;
      unsigned short int word_value;

      ROS_INFO("CDarwinSim : Getting current walk parameters ...");
      this->walk_access.enter();
      ram_read_byte(DARWIN_WALK_SWING_RIGHT_LEFT,&byte_value);
      res.params.Y_SWAP_AMPLITUDE=((double)byte_value)/1000.0;
      ram_read_byte(DARWIN_WALK_SWING_TOP_DOWN,&byte_value);
      res.params.Z_SWAP_AMPLITUDE=((double)byte_value)/1000.0;
      ram_read_byte(DARWIN_WALK_ARM_SWING_GAIN,&byte_value);
      res.params.ARM_SWING_GAIN=((double)byte_value)/32.0;
      ram_read_byte(DARWIN_WALK_PELVIS_OFFSET,&byte_value);
      res.params.PELVIS_OFFSET=(((double)byte_value)*PI/180.0)/8.0;
      ram_read_word(DARWIN_WALK_HIP_PITCH_OFF_L,&word_value);
      res.params.HIP_PITCH_OFFSET=(((double)word_value)*PI/180.0)/1024.0;
      ram_read_byte(DARWIN_WALK_X_OFFSET,&byte_value);
      res.params.X_OFFSET=((double)byte_value)/1000.0;
      ram_read_byte(DARWIN_WALK_Y_OFFSET,&byte_value);
      res.params.Y_OFFSET=((double)byte_value)/1000.0;
      ram_read_byte(DARWIN_WALK_Z_OFFSET,&byte_value);
      res.params.Z_OFFSET=((double)byte_value)/1000.0;
      ram_read_byte(DARWIN_WALK_YAW_OFFSET,&byte_value);
      res.params.A_OFFSET=(((double)byte_value)*PI/180.0)/8.0;;
      ram_read_byte(DARWIN_WALK_PITCH_OFFSET,&byte_value);
      res.params.P_OFFSET=(((double)byte_value)*PI/180.0)/8.0;
      ram_read_byte(DARWIN_WALK_ROLL_OFFSET,&byte_value);
      res.params.R_OFFSET=(((double)byte_value)*PI/180.0)/8.0;
      ram_read_word(DARWIN_WALK_PERIOD_TIME_L,&word_value);
      res.params.PERIOD_TIME=((double)word_value)/1000.0;
      ram_read_byte(DARWIN_WALK_DSP_RATIO,&byte_value);
      res.params.DSP_RATIO=((double)byte_value)/256.0;
      ram_read_byte(DARWIN_WALK_STEP_FW_BW_RATIO,&byte_value);
      res.params.STEP_FB_RATIO=((double)byte_value)/256.0;
      ram_read_byte(DARWIN_WALK_FOOT_HEIGHT,&byte_value);
      res.params.STEP_FB_RATIO=((double)byte_value)/1000.0;
      ram_read_byte(DARWIN_WALK_FOOT_HEIGHT,&byte_value);
      res.params.FOOT_HEIGHT=((double)byte_value)/1000.0;
      ram_read_byte(DARWIN_WALK_MAX_VEL,&byte_value);
      res.params.MAX_VEL=((double)byte_value)/1000.0;
      ram_read_byte(DARWIN_WALK_MAX_ROT_VEL,&byte_value);
      res.params.MAX_ROT_VEL=(((double)byte_value)*PI)/1440.0;
      this->walk_access.exit();

      return true;
    }
/************************************ get_walk_params *******************************************/

/************************************ set_servo_modules *****************************************/
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res)
    {
      unsigned int i=0,j=0;

      ROS_INFO("CDarwinSim : Setting servo modules ...");
      for(i=0;i<req.names.size();i++)
      {
        for(j=0;j<MANAGER_MAX_NUM_SERVOS;j++)
        {
          if(req.names[i]==servo_names[j])
          {
            if(req.modules[i]=="action")
              manager_set_module(j,MM_ACTION);
            else if(req.modules[i]=="walking")
              manager_set_module(j,MM_WALKING);
            else if(req.modules[i]=="joints")
              manager_set_module(j,MM_JOINTS);
            else if(req.modules[i]=="head")
              manager_set_module(j,MM_HEAD);
            else
              manager_set_module(j,MM_NONE);
            break;
          }
        }
      }
      res.ret=true;

      return true;
    }
/************************************ set_servo_modules *****************************************/

/************************************ set_pan_pid *****************************************/
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::set_pan_pidCallback(humanoid_common_msgs::set_pid::Request &req, humanoid_common_msgs::set_pid::Response &res)
    {
      uint16_t kp,ki,kd,i_clamp;
 
      ROS_INFO("CDarwinSim : Setting pan pid ...");
      kp=req.p*65536.0;
      ki=req.i*65536.0;
      kd=req.d*65536.0;
      i_clamp=((req.i_clamp*180.0)/3.14159)*128.0;
      head_tracking_set_pan_pid(kp,ki,kd,i_clamp);
      res.ret=true;

      return true;
    }
/************************************ set_pan_pid *****************************************/

/************************************ get_pan_pid *****************************************/
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::get_pan_pidCallback(humanoid_common_msgs::get_pid::Request &req, humanoid_common_msgs::get_pid::Response &res)
    {
      uint16_t kp,ki,kd,i_clamp;
 
      ROS_INFO("CDarwinSim : Getting pan pid ...");
      head_tracking_get_pan_pid(&kp,&ki,&kd,&i_clamp);
      res.p=((float)kp)/65536.0;
      res.i=((float)ki)/65536.0;
      res.d=((float)kd)/65536.0;
      res.i_clamp=((((float)i_clamp)*3.14159)/180.0)/128.0;
      res.ret=true;

      return true;
    }
/************************************ get_pan_pid *****************************************/

/************************************ set_tilt_pid *****************************************/
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::set_tilt_pidCallback(humanoid_common_msgs::set_pid::Request &req, humanoid_common_msgs::set_pid::Response &res)
    {
      uint16_t kp,ki,kd,i_clamp;
 
      ROS_INFO("CDarwinSim : Setting tilt pid ...");
      kp=req.p*65536.0;
      ki=req.i*65536.0;
      kd=req.d*65536.0;
      i_clamp=((req.i_clamp*180.0)/3.14159)*128.0;
      head_tracking_set_tilt_pid(kp,ki,kd,i_clamp);
      res.ret=true;

      return true;
    }
/************************************ set_tilt_pid *****************************************/

/************************************ get_tilt_pid *****************************************/
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::get_tilt_pidCallback(humanoid_common_msgs::get_pid::Request &req, humanoid_common_msgs::get_pid::Response &res)
    {
      uint16_t kp,ki,kd,i_clamp;
 
      ROS_INFO("CDarwinSim : Getting tilt pid ...");
      head_tracking_get_tilt_pid(&kp,&ki,&kd,&i_clamp);
      res.p=((float)kp)/65536.0;
      res.i=((float)ki)/65536.0;
      res.d=((float)kd)/65536.0;
      res.i_clamp=((((float)i_clamp)*3.14159)/180.0)/128.0;
      res.ret=true;

      return true;
    }
/************************************ get_tilt_pid *****************************************/

/****************************** get_smart_charger_config ***********************************/
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::get_smart_charger_configCallback(humanoid_common_msgs::get_smart_charger_config::Request &req, humanoid_common_msgs::get_smart_charger_config::Response &res)
    {
    }
/************************************ get_tilt_pid *****************************************/

/****************************** set_smart_charger_config ***********************************/
  template <class HardwareInterface>
    bool DarwinController<HardwareInterface>::set_smart_charger_configCallback(humanoid_common_msgs::set_smart_charger_config::Request &req, humanoid_common_msgs::set_smart_charger_config::Response &res)
    {
    }
/************************************ get_tilt_pid *****************************************/

/****************************************** imu *************************************************/
/****************************************** imu *************************************************/
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::gyro_callback(const sensor_msgs::Imu::ConstPtr& msg)
    {
      double gyro_x,gyro_y;

      // saturate to +/- 500 dps; 
      gyro_x=msg->angular_velocity.y*180.0/3.14159;
      if(gyro_x>500.0)
        gyro_x=500.0;
      else if(gyro_x<-500.0)
        gyro_x=-500.0;
      gyro_data[0]=(int32_t)(gyro_x*1000.0);
      gyro_y=msg->angular_velocity.z*180.0/3.14159;
      if(gyro_y>500.0)
        gyro_y=500.0;
      else if(gyro_y<-500.0)
        gyro_y=-500.0;
      gyro_data[1]=-(int32_t)(gyro_y*1000.0);
    }

  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::accel_callback(const sensor_msgs::Imu::ConstPtr& msg)
    {
      accel_data[0]=(int32_t)msg->linear_acceleration.x*1000.0;
      accel_data[1]=(int32_t)msg->linear_acceleration.y*1000.0;
      accel_data[2]=(int32_t)msg->linear_acceleration.z*1000.0;
    }
/****************************************** imu *************************************************/

/****************************************** cmd_vel *********************************************/
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
    {
      unsigned short int period;
      unsigned char step;

      this->walk_access.enter();
      ram_read_word(DARWIN_WALK_PERIOD_TIME_L,&period);
      ram_write_byte(DARWIN_WALK_STEP_FW_BW,(unsigned char)(msg->linear.x*period));
      ram_write_byte(DARWIN_WALK_STEP_LEFT_RIGHT,(unsigned char)(msg->linear.y*period));
      ram_write_byte(DARWIN_WALK_STEP_DIRECTION,(unsigned char)(((msg->angular.z*1.44)/PI)*period));
      if(!is_walking())
      {
        ROS_INFO("CDarwinSim : Start walking");
        walking_start();
      }
      this->walk_timeout=ros::Time::now();
      this->walk_access.exit();
    }
/****************************************** cmd_vel *********************************************/

/****************************************** head_target *************************************************/
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::head_target_callback(const trajectory_msgs::JointTrajectoryPoint::ConstPtr& msg)
    {
      ROS_INFO("CDarwinSim : New head target ...");
      if(msg->positions.size()==2)
      {
        head_tracking_set_target_pan(((msg->positions[0]*180.0)/3.14159)*128.0);
        head_tracking_set_target_tilt(((msg->positions[1]*180.0)/3.14159)*128.0);
      }
    }
/****************************************** head_target *************************************************/
 
  template <class HardwareInterface>
    DarwinController<HardwareInterface>::~DarwinController()
    {
      if(this->motion_action_aserver_!=NULL)
      {
        delete this->motion_action_aserver_;
        this->motion_action_aserver_=NULL;
      }
      if(this->joint_trajectory_aserver_!=NULL)
      {
        delete this->joint_trajectory_aserver_;
        this->joint_trajectory_aserver_=NULL;
      }
      if(this->follow_target_aserver_!=NULL)
      {
        delete this->follow_target_aserver_;
        this->follow_target_aserver_=NULL;
      }
    }

/****************************************** LEDS *************************************************/
  template <class HardwareInterface>
    void DarwinController<HardwareInterface>::leds_callback(const humanoid_common_msgs::leds::ConstPtr& msg)
    {
      unsigned int i=0;

      for(i=0;i<msg->name.size();i++)
      {
        if(msg->blink[i])
          ROS_INFO_STREAM("CDarwinSim : blink led " << msg->name[i] << " with period " << msg->period_ms[i] << " ms");
        else if(msg->toggle[i])
          ROS_INFO_STREAM("CDarwinSim : toggle led " << msg->name[i]);
        else if(msg->value[i])
          ROS_INFO_STREAM("CDarwinSim : set led " << msg->name[i]);
        else
          ROS_INFO_STREAM("CDarwinSim : clear led " << msg->name[i]);
      } 
    }
/****************************************** LEDS *************************************************/
} // namespace

#endif // header guard