ros之真实驱动diy6自由度机械臂（moveit中controller源码）

书接上回，

moveit 控制真实机器手臂时需要自己编写 控制器，控制器要有一个action server来接收 moveit的路径消息，然后控制器把消息下发到硬件上。

moveit 需要控制器获取并发布机机器手臂的状态。

此处创建两个节点，来实现这些功能。

第一个节点jointcontroller

1、负责 action server 功能，

2、路径消息转换成电机Id +角度r的一个新消息(joint_msg)，并发布/arm_motors的话题.

3、由于是舵机，并不能获取其位姿，获取部分省略。

4、发布舵机的当前位姿，即 从接收的路径消息来转换成joint_state消息，并发布/move_group/fake_controller_joint_states的话题。

第二个节点jointserial

接收话题/arm_motors，解析joint_msg 通过自定义协议串口下发。

自定义消息

joint_msg

int32 id
float64 r

jointcontroller.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

jointserial.cpp

#include 
#include 
//ROS已经内置了的串口包
#include 
#include 
#include "zzz_arm_control_driver/joint_msg.h"
#include                   //包含boost库函数


using namespace boost::asio;           //定义一个命名空间，用于后面的读写操作
using namespace std;
serial::Serial ser; //声明串口对象
//io_service Object
io_service m_ios;

//Serial port Object
serial_port *pSerialPort;

//For save com name
//any_type m_port;

//Serial_port function exception
boost::system::error_code ec;


std::string serial_port_name;
int serial_baudrate = 115200;
unsigned char AA=1;
unsigned char aa;
//回调函数
void write_callback(const zzz_arm_control_driver::joint_msg::ConstPtr& msg)
{
    ROS_INFO_STREAM("Writing to serial port" <id<r);
    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);
    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;
    string data="";
    data+=b1;
    data+=b2;
    data+=b3;

    //ser.write(data);//发送串口数据
    try
    {
      size_t len = write( *pSerialPort, buffer( cdata,3 ), ec );
    }catch (boost::system::system_error e){
        ROS_ERROR_STREAM("serail write err ");

    }
}

int main (int argc, char** argv)
{

    //初始化节点
    ros::init(argc, argv, "jointserial");
    //声明节点句柄
    ros::NodeHandle nh;

    //订阅主题，并配置回调函数
    ros::Subscriber write_sub = nh.subscribe("/arm_motors", 1000, write_callback);
    //发布主题
    //ros::Publisher read_pub = nh.advertise("read", 1000);
    ros::NodeHandle nh_private("~");
    nh_private.param("serial_port", serial_port_name, "/dev/ttyUSB0");
    nh_private.param("serial_baudrate", serial_baudrate, 115200);

    /*try
    {
    //设置串口属性，并打开串口

      ROS_INFO("%s",serial_port_name.c_str());
      ser.setPort(serial_port_name);
      ser.setBaudrate(serial_baudrate);
      serial::Timeout to = serial::Timeout::simpleTimeout(10000);
      ser.setTimeout(to);
      ser.open();

    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }
    //检测串口是否已经打开，并给出提示信息
    if(ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else
    {
        return -1;
    }//*/
    pSerialPort = new serial_port( m_ios );
    if ( pSerialPort ){
        //init_port( port_name, 8 );
        //Open Serial port object
        pSerialPort->open( serial_port_name, ec );
        //Set port argument
        pSerialPort->set_option( serial_port::baud_rate( serial_baudrate ), ec );
        pSerialPort->set_option( serial_port::flow_control( serial_port::flow_control::none ), ec );
        pSerialPort->set_option( serial_port::parity( serial_port::parity::none ), ec );
        pSerialPort->set_option( serial_port::stop_bits( serial_port::stop_bits::one ), ec);
        pSerialPort->set_option( serial_port::character_size( 8 ), ec);
        m_ios.run();
    }

    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 endia\n");
    } else {
        ROS_INFO("unknown endia\n");
    }

    ROS_INFO("-------------zzz joint serail is running .");
    ros::spin();

    return 0;

/*
    //指定循环的频率
    ros::Rate loop_rate(50);
    while(ros::ok())
    {

        if(ser.available()){
            ROS_INFO_STREAM("Reading from serial port\n");
            std_msgs::String result;
            result.data = ser.read(ser.available());
            ROS_INFO_STREAM("Read: " << result.data);
            read_pub.publish(result);
        }

        //处理ROS的信息，比如订阅消息,并调用回调函数
        ros::spinOnce();
        loop_rate.sleep();

    }*/
}