开发ROS 程序包控制机械臂

ROS（Robot Operation System）是一个机器人软件平台，提供一些标准操作系统服务，例如硬件抽象，底层设备控制，常用功能实现，进程间消息以及数据包管理。ROS是基于一种图状架构，从而不同节点的进程能接受，发布，聚合各种信息（例如传感，控制，状态，规划等等）。目前ROS主要支持Ubuntu。ROS（低层）使用BSD许可证，所有都是开源软件，并能免费用于研究和商业用途。由于其强大的功能、全面的信息及方便快捷的开发，使得开源的ROS逐渐成为机器人开发者首选的开发工具和平台。更多学习内容详见网页 http://wiki.ros.org/cn。

为了学习ROS系统，实现ROS系统对单片机的数据通讯和控制，试着通过在Ubuntu虚拟机下编写ROS程序包，使用键盘（也可以是鼠标）控制UARM机械臂。这样，原本控制单片机靠的是在Windows系统上安装的鼠标控制程序MouseControl_V1.0.5.exe，现在要被在Ubuntu系统下运行的ROS程序包所取代。原先已经烧写到单片机内的程序不作更改。

这部分工作是在一位资深搞ROS的小伙伴帮助指导下完成的。在ROS下建立两个节点（NODE）或程序包（PACKAGE）和一个话题（TOPIC），第一个节点负责捕捉电脑键盘信息，并向话题发送消息（MESSAGE）；话题收到消息后向外发送；第二个节点接收话题传过来的消息，并向单片机串口发送数据。每帧数据含11个字节。在本项目中，节点间传输的消息为按键按下所表征的机械臂运动信息。

ROS程序编写前需要先获取UARM官方所提供的程序中单片机串口通讯、舵机控制等的相关信息。

在ROS里主要编写三个程序代码：消息发送程序包ros_uarm.cpp、消息接收程序包get_msg.cpp和话题uarm_keyboard.msg。他们的代码如下：

1) ros_uarm.cpp：

#include <ros/ros.h>
#include <keyboard_resolve/uarm_keyboard.h>
#include "UF_ARM.h"
#include "serial/serial.h"
#define delta_increment 2;
#define rotate_delta_increment 1;
std::string port;
std::string keyboard_topic_;
int32_t baud;
serial::Serial ser;
struct motor_velocity
{
int16_t height;
int16_t stretch;
int16_t rotate;
int16_t hand;
};
motor_velocity arm_robot;
uint8_t SerialBuf[11];
void keyboardCallback(const keyboard_resolve::uarm_keyboard& omni_velocity)
{

if (omni_velocity.hand_catch)
{
SerialBuf[10] = 1;
}
if (omni_velocity.hand_release)
{
SerialBuf[10] = 2;
}
if (omni_velocity.height_down || omni_velocity.height_up)
{
if (omni_velocity.height_down)
{
arm_robot.height -= delta_increment;
}
if (omni_velocity.height_up)
{
arm_robot.height += delta_increment;
}
if (arm_robot.height < ARM_HEIGHT_MIN)
{
arm_robot.height = ARM_HEIGHT_MIN;

}
else if (arm_robot.height > ARM_HEIGHT_MAX)
{
arm_robot.height = ARM_HEIGHT_MAX;
}
SerialBuf[7] = arm_robot.height & 0xff;
SerialBuf[6] = (arm_robot.height >> 8) & 0xff;
}
if (omni_velocity.rotate_left || omni_velocity.rotate_right)
{
if (omni_velocity.rotate_left)
{
arm_robot.rotate -= rotate_delta_increment;
}
if (omni_velocity.rotate_right)
{
arm_robot.rotate += rotate_delta_increment;
}
if (arm_robot.rotate < ARM_ROTATION_MIN)
{
arm_robot.rotate = ARM_ROTATION_MIN;

}
else if (arm_robot.rotate > ARM_ROTATION_MAX)
{
arm_robot.rotate = ARM_ROTATION_MAX;
}
SerialBuf[3] = arm_robot.rotate & 0xff;
SerialBuf[2] = (arm_robot.rotate >> 8) & 0xff;
}
if (omni_velocity.stretch_back || omni_velocity.stretch_up)
{
if (omni_velocity.stretch_back)
{
arm_robot.stretch -= delta_increment;
}
if (omni_velocity.stretch_up)
{
arm_robot.stretch += delta_increment;
}
if (arm_robot.stretch < ARM_STRETCH_MIN)
{
arm_robot.stretch = ARM_STRETCH_MIN;

}
else if (arm_robot.stretch > ARM_STRETCH_MAX)
{
arm_robot.stretch = ARM_STRETCH_MAX;
}
SerialBuf[5] = arm_robot.stretch & 0xff;
SerialBuf[4] = (arm_robot.stretch >> 8) & 0xff;
}
if (omni_velocity.reset)
{
arm_robot.stretch = 0;
arm_robot.rotate = 0;
arm_robot.height = 0;
SerialBuf[0] = 0xFF;
SerialBuf[1] = 0xAA;
SerialBuf[3] = arm_robot.rotate & 0xff;
SerialBuf[2] = (arm_robot.rotate >> 8) & 0xff;
SerialBuf[5] = arm_robot.stretch & 0xff;
SerialBuf[4] = (arm_robot.stretch >> 8) & 0xff;
SerialBuf[7] = arm_robot.height & 0xff;
SerialBuf[6] = (arm_robot.height >> 8) & 0xff;
SerialBuf[8] = 0;
SerialBuf[9] = 0;
SerialBuf[10] = 0;
}
std::cout<<arm_robot.hand<<"\n"<<arm_robot.height<<"\n"<<arm_robot.rotate<<"\n"<<arm_robot.stretch<<"\n";
ser.write(SerialBuf, 11);
std::cout << omni_velocity << "\n";
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "get_msg");
ros::NodeHandle node;
node.param<std::string>("keyboard_topic", keyboard_topic_, "uarm_keyboard");
node.param<std::string>("serial_port", port, "/dev/ttyUSB0");
node.param<int32_t>("baud",baud,9600);
ser.setPort(port);
ser.setBaudrate(baud);
serial::Timeout to = serial::Timeout(50, 50, 0, 50, 0);
ser.setTimeout(to);
if (ser.isOpen())
{
ser.close();
}
ros::Rate wait_serial(1);
int time_count = 0;
while (!ser.isOpen()&&ros:k())
{
try
{
ser.open();
}
catch (const std::exception& e)
{
ROS_ERROR("Unable to connect to port.");
}
time_count++;
ROS_INFO("wait for about %ds", time_count);
wait_serial.sleep();
}
if (ser.isOpen())
{
ROS_INFO("Successfully connected to serial port.");

}
arm_robot.stretch = 0;
arm_robot.rotate = 0;
arm_robot.height = 0;
SerialBuf[0] = 0xFF;
SerialBuf[1] = 0xAA;
SerialBuf[3] = arm_robot.rotate & 0xff;
SerialBuf[2] = (arm_robot.rotate >> 8) & 0xff;
SerialBuf[5] = arm_robot.stretch & 0xff;
SerialBuf[4] = (arm_robot.stretch >> 8) & 0xff;
SerialBuf[7] = arm_robot.height & 0xff;
SerialBuf[6] = (arm_robot.height >> 8) & 0xff;
SerialBuf[8] = 0;
SerialBuf[9] = 0;
SerialBuf[10] = 0;
ser.write(SerialBuf, 11);
ros::Subscriber sub = node.subscribe(keyboard_topic_, 10, keyboardCallback);//subsribe the topic
ros::spin();
arm_robot.stretch = 0;
arm_robot.rotate = 0;
arm_robot.height = 0;
SerialBuf[0] = 0xFF;
SerialBuf[1] = 0xAA;
SerialBuf[3] = arm_robot.rotate & 0xff;
SerialBuf[2] = (arm_robot.rotate >> 8) & 0xff;
SerialBuf[5] = arm_robot.stretch & 0xff;
SerialBuf[4] = (arm_robot.stretch >> 8) & 0xff;
SerialBuf[7] = arm_robot.height & 0xff;
SerialBuf[6] = (arm_robot.height >> 8) & 0xff;
SerialBuf[8] = 0;
SerialBuf[9] = 0;
SerialBuf[10] = 0;
ser.write(SerialBuf, 11);
return 0;
}

2) uarm_keyboard.msg：

bool rotate_left

bool rotate_right

bool stretch_up

bool stretch_back

bool height_up

bool height_down

bool hand_catch

bool hand_release

bool reset