ros之真实驱动diy6自由度机械臂

x宝淘的6轴手臂一直在吃土。
萌生一念，不如试着用ros来驱动它吧。
量了关节的位置，想象着对比着一个测试模型，手动写好了模型文件 实际上是很多xml的xacro。
写个launch 方便反复调试造型和关节限位。

<launch>
   <!-- Load URDF into ROS param server -->
  <param name="robot_description" command="$(find xacro)/xacro '$(find zzz_arm_description)/robots/zzz_arm_base.urdf.xacro'"/>
<param name="use_gui" value="true"/>
  <!-- Joint limits -->
  <rosparam file="$(find zzz_arm_description)/config/joint_limits.yaml" command="load"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" /><!--args="-d $(find robot1)/urdf.rviz" -->
</launch>

有了好的模型之后调用
 

roslaunch moveit_setup_assistant setup_assistant.launch

moveit可视化设置手臂分组、规划位置等生成一个包。细节不表。
完成后launch一下

roslaunch packge demo.launch

启动rviz测试规划。

此时，手臂可以自动移动到给定的目标。看起来只要弄清楚moveit的通讯协议，写个节点监听并改造成自己硬件的协议串口下发便大功告成。



看看moveit官方介绍

http://moveit.ros.org/documentation/concepts/

Controller Interface

move_group talks to the controllers on the robot using the FollowJointTrajectoryAction interface. This is a ROS action interface. A server on the robot needs to service this action - this server is not provided by move_group itself. move_group will only instantiate a client to talk to this controller action server on your robot.
概述里对控制接口的介绍
moveit跟控制器对话是通过FollowJointTrajectoryAction接口，就是说规划的结果会以一个action的形式发布，move_group不提供action sever而只有 client.所以我们应该打造一个action server.
代码
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> server;
action的类型是FollowJointTrajectory
需要在自定义的_controllers.yaml配置type

看起来路子很简单。
just do it!!
然后我照猫画虎写好了一个server的 node，改写了moveit自动生成的 yaml。

该有的都有了，似乎可以了，但是启动起来为啥action client 和 server 不能成功通讯来？？

顺着报错捋一捋：

问题
Unable to identify any set of controllers that can actuate the specified joints: [.....]
不能识别能驱动指定关节的控制器的任何设置.....翻译菜，比较绕口。说白了就是没有找到我们对控制器的配置。


不了解moveit包如何配置控制器，云里雾里的，度娘度娘

解答
https://answers.ros.org/question/167501/how-to-solve-parameter-moveit_controller_manager-not-specified/


Did you create the moveit config by yourself for your robot Sam, using the setup assistant for instance or by hand? If yes, using the setup assistant is not enough to use Moveit, you need to provide Moveit a way to control your robot, through a controller manager.

By default, the setup assistant creates a fake_controller_manager. See the default fake_controller_manager.launch.xml file in the launch/ directory. You my create your own controller manager, or check if it's not already in the package moveit_robots.

This controller manager is launched in trajectory_execution.launch.

大概意思说光使用setup assistant is not enough，它只生成了一个虚拟的控制器fake_controller_manager， 这里需要定义自己的一个控制器。需要理清 assistant生成的每个文件的作用，修改并配置之。
好吧，先辈说是trajectory_execution.launch，我这里没有这个文件但有一个
trajectory_execution.launch.xml
看文件内部调用了

<include file="$(find zzz_arm_moveit)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />

直觉上应该是修改
×_moveit_controller_manager.launch.xml
网上说就这么写

<launch>
    <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

    <!-- load controller_list -->
    <arg name="use_controller_manager" default="true" />
    <param name="use_controller_manager" value="$(arg use_controller_manager)" />

    <!-- Load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(find zzz_arm_moveit)/config/自定义的_controllers.yaml"/>

</launch>

好吧，我也这么写。



关联上自定义的_controllers.yaml
它是这个样式的

controller_list:
  - name: zzz_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - shoulder_wan_joint
      - upper_arm_joint
      - middle_arm_joint
      - fore_arm_joint
      - tool_joint
  - name: zzz_grapper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - finger_1_joint
      - finger_2_joint

t


注意的点，前文说过为什么type这么写
  - name: zzz_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory


在node cpp源码里应该如下对应

typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;

Server server(n, "zzz_arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false);

这些工作做好运行demo.launch和server node故障依旧，还是不行，我的一口老血呀，抽电脑的心都有了！！
别乱别乱，莫慌莫慌，荒了不打粮。细节，一定注意细节～～，配置一定是没起作用。
仔细找寻嵌套关系看怎样才能执行到我们在×_moveit_controller_manager.launch.xml的文件？
嵌套关系：
demo.launch
    >move_group.launch
        >trajectory_execution.launch.xml
            >×_moveit_controller_manager.launch.xml

demo.launch中有如下代码段

<include file="$(find zzz_arm_moveit)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="false"/><!-- zzz -->
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

fake_execution 参数设置为 false 才能执行到×_moveit_controller_manager.launch.xml


捋到此处值得庆幸的是client和server 终于搞基成功了。



问题
Controller is taking too long to execute trajectory
解答
https://answers.ros.org/question/196586/how-do-i-disable-execution_duration_monitoring/

The following should work in trajectory_execution.launch.xml

<param name="trajectory_execution/execution_duration_monitoring" value="false" />

Instead of disabling execution duration monitoring you can increase the time for your manipulator.

<param name="allowed_execution_duration_scaling" value="3.0"/>




http://wiki.ros.org/fanuc/Troubleshooting/MoveIt%20Trajectory%20Execution%20Manager

The trajectory execution manager waits for the amount of time declared by moveit_msgs::RobotTrajectory for the execution to complete. If that time is exceeded by a significant margin (more than 10% longer), the execution is cancelled. This behaviour can be disabled by calling enableExecutionDurationMonitoring(false) for the trajectory execution manager instance or by setting the ROS parameter ~/execution_duration_monitoring (e.g., as part of the move_group node). To scale the amount of time to wait (e.g., more than 10%), the setAllowedExecutionDurationScaling() function can be called or the ROS parameter ~/allowed_execution_duration_scaling can be set




问题的问题
action sever 的 callback，C++好高深，半天没弄明白。

typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;
//类成员
Server as_;
//初始关联带参数callback成功
   as_(nh_, name,boost::bind(&RobotTrajectoryFollower::goalCB, this, _1),  false),
//注册关联带参数，解决方法？？
   //as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this,_1));
    
void goalCB(const control_msgs::FollowJointTrajectoryGoalConstPtr msg)
{
//这个callback的参数类型如何确定？
}

问题
 failed to receive current joint states
问题
每次规划起点总是从原点



解答
认真rqt_graph一下，对比好的demo确定是关节状态的没有发布的问题。

自定义的controller驱动真实手臂时需要自己获取手臂的位姿数据，并发布出去
根据demo.launch的这段代码段分析，publisher的topic名称应该为/move_group/fake_controller_joint_states
 

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="$(arg use_gui)"/>
    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
  </node>

c++代码
 

  ros::Publisher joint_pub ;
  sensor_msgs::JointState joint_state;



  joint_pub = nh_.advertise<sensor_msgs::JointState>("/move_group/fake_controller_joint_states", 1);

对于舵机手臂的位姿，直接发布规划路径的各个点数据就好了
就是在goalCB内把数据取出来，再用publisher 发布出去。

问题

硬件arduino驱动

舵机控制 正好有可用的代码

http://blog.csdn.net/qq_38288618/article/details/76626398
iic协议扩展板和pmw引脚控制舵机转向
需要注意的是控制舵机的pmw要根据实际舵机使用的脉宽范围重新计算一下

void sendToIIC(byte _AAA,byte _aaa,unsigned int _ddd){  
  //50hz 相当于20ms(20000us)的 周期,  4096分辨率，分辨率每单位占用时间t 为20000us/4096=4.8828125us  
  //舵机控制所需脉宽Th范围0.5~2.5ms（500~2500us）因此高电平分辨率范围就是Th/t =(102.4~512)个单位。  
  //取整 最小103 最大512 范围是512-103=409；  
  //pwm.setAddr(0x40+_AAA);  
  float per=(float)_ddd/1024.0;//参数ddd为10位正整数最大1024  
  uint16_t offNum=103+int(409*per);  
  //pwm.setPWM(num,on,off);num pmw的id代号,on 上升沿（在0-4096之间）开始位置,off 下降沿在（在0-4096之间）的开始位置  
  pwm.setPWM(_aaa, 0, offNum);  
 
}

uint16_t offNum=103+int(409*per);应该改成uint16_t offNum=205+int(205*per);
205是怎么来的？
因为 所需脉宽范围是1ms到2ms 1.5ms归中。1000/t=204.8 2000/t=409.6 差值范围204.8 所以取整得到205.



ros模型设置关节转动范围在正负之间时，处理一下偏移。我只有一个关节 mid==1的关节。
实际电机安装正转反转方向的设置。除了(mid==1||mid==5)的这两个电机，其他都改成反向。
示例代码：

    unsigned char mid=(char) msg->id;
    double r=msg->r;
    if(mid==1){
      r+=3.1415926536/4.0;//motor1 limit:-45~45
    }
    r=r<0.0?0.0:r;
    r=r>3.1415926/2.0?3.1415926/2.0:r;
    double per=r/(3.1415926/2.0);

    per=(mid==1||mid==5)?per:(1-per);

实际项目应该花点时间改成 外部parm进行配置的模式。



自定义串口协议发送short数据,总是不对，结果是遇到了 little endian还是big endian的问题
检测代码考过来

short int a = 0x1234;
    char *p = (char *)&a;

    ROS_INFO("p=%#hhx\n", *p);

    if (*p == 0x34) {
        ROS_INFO("little endian\n");
    } else if (*p == 0x12) {
        ROS_INFO("big endian\n");
    } else {
        ROS_INFO("unknown endia\n");
    }

开始想当然的认为高位在前 低位在后的顺序，疏忽了一下还是被电脑耍了。
测试结果是 big endian
发送short数据时下面的代码
mrp+1先取short的第二个字节 再取第一个字节。协议按照自己的需要改

unsigned short  mr=(unsigned short )((per)*1024.0);

    ROS_INFO("id:%d per:%d",mid,mr);
    unsigned char* mrp=  (unsigned char*)&mr;

    unsigned char b1,b2,b3;
    //11aa aaAA
    b1=0xc0|(mid<<2);
    b1|=0x03&(AA>>2);
    //10AA xxxx
    b2=0x80|(AA<<4);
    b2|=(0x0c&((*(mrp+1))<<2))|(0x03&((*(mrp))>>6));
    //01xx xxxx

    b3=0x40|(0x3f&(*(mrp)));
    char cdata[3];
    cdata[0]=b1;
    cdata[1]=b2;
    cdata[2]=b3;

上两张效果图，

廉价的硬件，为了方便加了个蓝牙通讯模块。

总结和反思一下

写代码需要穿针引线的细致。
路淌过，方知坑。