ros controller控制diy手臂抓手开合，利用service实现角位距离相互转换的思路,实现真实抓取放置 sevice源码

moveit规划实现抓取放置的现场

rqtgraph

由于DIY的手臂上的抓手一般是一只舵机控制的夹子（两个手指对称移动的机械结构）

构建urdf模型时一般是简单的定义两个手指为滑动关节。
抓手规划一般是指定开合距离，即左右手指的位置

这就要求我们需要把电机的旋转角位和开合距离做一个转换的功能。
要写一个抓手的controller或是驱动，都应该需要写一个简单映射或者动力学的公式才行。


思路是创建一个service
ros service 好处是实时、一对多。这样我们在驱动和controller里都可以调用了。


消息结构如下

int8 flag
float64 request
---
float64 response

flag是标记 取值 1,2
1时 request 是角位 response是距离
2时 request 是距离 response是角位


驱动读出舵机的角位实际是个pwm值，用最土的映射办法，准备一系列pwm值，测到与之对应的夹子宽度，之间进行线性插值。
下面实现转换功能的service源码

#include "ros/ros.h"
#include "zzz_arm_2_srv_finger/trans_state.h"

unsigned char const len=17;
double  pwm[len]={
  1350.0,1400.0,1450.0,1500.0,1550.0,1600.0,1650.0,1700.0,1750.0,
  1800.0,1850.0,1900.0,1950.0,2000.0,2050.0,2100.0,2200.0
};
double dis[len]={
  0.0,0.003,0.0045,0.01,0.012,0.016,0.018,0.022,0.025,
  0.032,0.034,0.038,0.042,0.047,0.048,0.052,0.053
};
bool trans(zzz_arm_2_srv_finger::trans_state::Request  &req,
         zzz_arm_2_srv_finger::trans_state::Response &res)
{
  int i=0;
  double p=0.0;
  if(req.flag==1){
    //pwm2dis r2d
    for(i=0;i<len-1;i++){
      if(req.request>=pwm[i]&&req.request<=pwm[i+1]){
        p=(req.request-pwm[i])/(pwm[i+1]-pwm[i]);
        break;
      }
    }
    p=p<0?0:p;
    p=p>1?1:p;
    res.response = dis[i]+p*(dis[i+1]-dis[i]);
  }else if(req.flag==2){
    //dis2pwm d2r
    for(i=0;i<len-1;i++){
      if(req.request>=dis[i]&&req.request<=dis[i+1]){
        p=(req.request-dis[i])/(dis[i+1]-dis[i]);
        break;
      }
    }
    p=p<0?0:p;
    p=p>1?1:p;
    res.response = pwm[i]+p*(pwm[i+1]-pwm[i]);

  }


  ROS_INFO("request: flg=%d, req=%lf", ( int)req.flag, (double)req.request);
  ROS_INFO("response: res=[%lf]", (double)res.response);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "zzz_arm_2_trans_state");
  ros::NodeHandle nh;

  ros::ServiceServer service = nh.advertiseService("zzz_arm_finger_state_trans", trans);
  ROS_INFO("ServiceServer zzz_arm_finger_state_trans Ready to trans finger's state.");
  ros::spin();

  return 0;
}

驱动里使用service
urdf里这样设置两个手指坐标为0,夹子距离为0
对称移动，距离/2=abs(坐标)

驱动里读出pwm值call service得到距离,计算坐标并发布手指的joint_states
moveit和controller都要用到这个 joint_states

srv.request.flag = 1;
    srv.request.request = (double) posi;

    if (client.call(srv))
    {
      ROS_INFO("griper dis: %lf", (double)srv.response.response);
      double dis=srv.response.response/2;
      joint_state_msg.header.stamp = ros::Time::now();
      joint_state_msg.name.resize(2);
      joint_state_msg.position.resize(2);

      joint_state_msg.name[0] ="finger_1_joint";
      joint_state_msg.position[0] = -dis ;
      joint_state_msg.name[1] ="finger_2_joint";
      joint_state_msg.position[1] = dis ;


      //publish
      joint_pub_arm.publish(joint_state_msg);
      ros::spinOnce();

    }
    else
    {
      ROS_ERROR("r2d Failed to call service zzz_arm_finger_state_trans");
      //return 1;
    }

controller里轨迹（一系列坐标）控制的转换

两个坐标绝对值相加得到距离，call service 得到pwm 最终转换为真实电机的角位（弧度）。
发布消息控制电机运动

void GrpHardwareInterface::write(ros::Duration elapsed_time)
{
  // Enforce joint limits
  //jnt_limits_interface_.enforceLimits(elapsed_time);
  zzz_arm_2_control_driver_msgs::joint_msg joint_motor_msg;


    // Compute and send commands
    double dis=0.0;
    char flag=0;
    for (size_t i = 0; i < n_dof_; ++i)
    {
      dis+=fabs(joint_position_command_[i]);
      if(joint_position_command_last_[i]!=joint_position_command_[i]){
        flag=1;
        joint_position_command_last_[i]=joint_position_command_[i];
      }
    }

//*
      //zzz_arm_2_srv_finger::trans_state srv;
    if(flag==1){
      srv.request.flag = 2;
      srv.request.request =  dis;
      if (client.call(srv))
      {
        //ROS_INFO("griper r: %lf", (double)srv.response.response);
        double pwm=srv.response.response;//test 2000.0;//
        double per=double (pwm-1500.0)/1000.0;
        double pi=3.1415926535;
        double r=per*pi/4.0*3.0;

        joint_motor_msg.id=5;
        joint_motor_msg.r=r;
        joint_motor_msg.t=elapsed_time.toSec()*2.0;
        joint_motor_pub_.publish(joint_motor_msg);
        ros::spinOnce();
      }
      else
      {
        ROS_ERROR("d2r Failed to call service zzz_arm_finger_state_trans");
        //return 1;
      }//*/
    }


}

有了这个service ,按照示例在抓手的controller 串口驱动加以利用， 就可以进行手臂的抓取和放置操作了。如果再加个kinect利用点云和图像识别环境和抓取物体，完成全自动抓放那就更完美了！

……要什么自行车？


问题

could not find joint 'xxx' in 'hardware_interface::PositionJointInterface'
initializing controller 'xxx' failed

分析
两个controller
照猫画虎，写两个类继承public hardware_interface::RobotHW
每个类都有
hardware_interface::PositionJointInterface pj_interface_;
分别注册
registerInterface(&pj_interface_);
按照套路使用 controllermanager
编译两个节点
每个节点单独运行（只有一个controller）时正常，两个一起则报上边的错误。
问题似乎出在registerInterface
虽然是在两个节点分别注册，但 似乎是覆盖了全局的数据？
还是琢磨不透写ros_controll 作者的心思



解决
最终合二为一，只registerInterface一次，在一个节点里搞定就ok了。

不过，总觉的这样不符合ros框架分而治之的思想，求大神赐教！

ros service 一对多。
http://blog.csdn.net/pcler/article/details/45587313

Linux环境下段错误的产生原因及调试方法小结
https://www.cnblogs.com/panfeng412/archive/2011/11/06/2237857.html


ROS学习（十三）：time 和 Timer

http://blog.csdn.net/xuehuafeiwu123/article/details/68060377