通过ROS的Message控制舵机角度(C++)¶
安装依赖¶
需要在Ubuntu上安装FashionStar总线伺服舵机 C++ SDK
参见教程:
创建包¶
ROS Wiki - Creating a ROS Package
进入文件夹
创建包
创建Message¶
官方文档参考: ROS wiki - msg
创建文件夹¶
在fashionstar_uart_servo_ros1
工程内新建msg
文件夹,放置Message
创建Message文件¶
标准信息格式支持如下
- int8,int16,int32,int64
- float32,float64
- string
- 其他自定义msg:package/MessageName
- 不定长array[ ]以及定长array[N]
创建一个自己的Message格式,
SetServoAngle.msg
配置pakcage.xml
¶
添加如下两行:
构建的时候生成message的依赖,
运行的时候message依赖,
配置CMakeLists.txt
¶
配置find_package
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation # Message生成依赖
)
注册.msg
文件
## Generate messages in the 'msg' folder
## 这里添加msg文件夹下的msg文件名称
add_message_files(
FILES
SetServoAngle.msg
)
添加生成message的时候需要的依赖
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
配置catkin, CATKIN_DEPENDS
里面添加message_runtime
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES fashionstar_uart_servo_ros1
CATKIN_DEPENDS message_runtime roscpp rospy std_msgs
DEPENDS system_lib
)
构建Message¶
使用rosmsg查看创建的msg¶
指令格式
示例:
kyle@turing:~/catkin_ws$ rosmsg show SetServoAngle
[fashionstar_uart_servo_ros1/SetServoAngle]:
uint8 id
float32 angle
C++节点-消息接收者¶
在src
下创建uservo_demo_node.cpp
/*
* 舵机控制节点(Demo)
*/
// 导入ROS依赖
#include "ros/ros.h"
#include "fashionstar_uart_servo_ros1/SetServoAngle.h"
#include "CSerialPort/SerialPort.h"
#include "FashionStar/UServo/FashionStar_UartServoProtocol.h"
#include "FashionStar/UServo/FashionStar_UartServo.h"
using namespace fsuservo;
using namespace fashionstar_uart_servo_ros1;
// 参数定义
#define SERVO_PORT_NAME "/dev/ttyUSB0" // Linux下端口号名称 /dev/ttyUSB{}
#define SERVO_ID 0 // 舵机ID号
// 创建协议对象
FSUS_Protocol protocol(SERVO_PORT_NAME, FSUS_DEFAULT_BAUDRATE);
// 创建一个舵机对象
FSUS_Servo servo0(SERVO_ID, &protocol);
/* 舵机角度设置回调函数 */
void set_servo_angle_callback(const SetServoAngle& data){
ROS_INFO("[RECV] Servo ID = %d Set Angle = %.1f", data.id, data.angle);
// 设置舵机角度
servo0.setRawAngle(data.angle, 0);
}
int main(int argc, char **argv)
{
// 创建节点名称
ros::init(argc, argv, "uservo_demo_node");
// 创建NodeHandle
ros::NodeHandle node_handle;
// 创建接收者
ros::Subscriber set_servo_angle_sub = node_handle.subscribe("set_servo_angle", 2, set_servo_angle_callback);
// 进入循环等待
ros::spin();
}
C++节点-消息发送者¶
在src
下创建test_set_angle_node.cpp
/*
* 测试舵机角度控制节点
*/
#include "ros/ros.h"
// 自定义消息
#include "fashionstar_uart_servo_ros1/SetServoAngle.h"
using namespace fashionstar_uart_servo_ros1;
#define SERVO_ID 0 // 舵机ID号
int main(int argc, char **argv)
{
// 创建节点名称
ros::init(argc, argv, "test_set_angle_node");
// 创建NodeHandle
ros::NodeHandle node_handle;
// 创建发布者
ros::Publisher set_servo_angle_pub = node_handle.advertise<SetServoAngle>("set_servo_angle", 2);
// 循环频率0.2HZ
ros::Rate loop_rate(0.2);
// 创建Message
SetServoAngle msg;
float angle = 90.0;
while(ros::ok()){
// 修改目标角度
angle *= -1.0;
// 构建Message
msg.id = SERVO_ID;
msg.angle = angle;
// 发布消息
set_servo_angle_pub.publish(msg);
// 输出日志
ROS_INFO("Set Servo %d Angle = %.1f", msg.id, msg.angle);
// 延时等待
loop_rate.sleep();
}
}
配置CMakeLists.txt
¶
配置CMakeLists.txt
添加include
路径,这样才可以找到依赖库的.h
文件所在的路径
set(CMAKE_INSTALL_PREFIX /usr/local)
include_directories(
# include
${catkin_INCLUDE_DIRS}
${CMAKE_INSTALL_PREFIX}/include
)
设置link
文件夹, 动态链接库所在的位置
添加可执行程序
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
## 定义C++可执行文件
# - 总线伺服舵机服务示例节点
add_executable(uservo_demo_node src/uservo_demo_node.cpp)
add_executable(test_set_angle_node src/test_set_angle_node.cpp)
动态链接
## Specify libraries to link a library or executable target against
target_link_libraries(uservo_demo_node
${catkin_LIBRARIES}
cserialport # 轻量级串口通信库
fsuartservo # FashionStar总线伺服舵机SDK (C++)
)
target_link_libraries(test_set_angle_node
${catkin_LIBRARIES}
)
修改完成的CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(fashionstar_uart_servo_ros1)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation # Message生成依赖
)
################################################
## Declare ROS messages, services and actions ##
################################################
## Generate messages in the 'msg' folder
## 这里添加msg文件夹下的msg文件名称
add_message_files(
FILES
SetServoAngle.msg # 设置舵机角度
SetServoDamping.msg # 设置舵机为阻尼模式
)
## Generate services in the 'srv' folder
add_service_files(
FILES
QueryServoAngle.srv # 舵机角度查询服务
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
###################################
## catkin specific configuration ##
###################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES fashionstar_uart_servo_ros1
CATKIN_DEPENDS message_runtime roscpp rospy std_msgs
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
set(CMAKE_INSTALL_PREFIX /usr/local)
include_directories(
# include
${catkin_INCLUDE_DIRS}
${CMAKE_INSTALL_PREFIX}/include
)
# 设置总线伺服舵机链接库的位置
link_directories(
${CMAKE_INSTALL_PREFIX}/lib
)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
## 定义C++可执行文件
# - 总线伺服舵机服务示例节点
add_executable(uservo_demo_node src/uservo_demo_node.cpp)
add_executable(test_set_angle_node src/test_set_angle_node.cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(uservo_demo_node
${catkin_LIBRARIES}
cserialport # 轻量级串口通信库
fsuartservo # FashionStar总线伺服舵机SDK (C++)
)
target_link_libraries(test_set_angle_node
${catkin_LIBRARIES}
)
运行例程¶
运行Master节点
运行舵机服务节点(接收者)
运行舵机角度控制测试节点(发送者)