ROS主题发布订阅控制真实的机器人下位机
先模拟控制小乌龟
新建cmd_node.ccpp文件:
#include"ros/ros.h" #include"geometry_msgs/Twist.h" //包含geometry_msgs::Twist消息头文件 #include <stdlib.h> #include<stdlib.h> int main(int argc, char **argv) { ros::init(argc, argv, "cmd_node"); ros::NodeHandle n; ros::Publisher cmd_pub = n.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",1000); ros::Rate loop_rate(10); while(ros::ok()) { geometry_msgs::Twist msg; msg.linear.x = (double)(rand()/(double(RAND_MAX))); //随机函数 msg.angular.z = (double)(rand()/(double(RAND_MAX))); cmd_pub.publish(msg); ROS_INFO("msg.linear.x:%f , msg.angular.z: %f",msg.linear.x,msg.angular.z); loop_rate.sleep(); } return 0; }
编译成功产生
测试
roscore
rosrun turtlesim turtlesim_node
rosrun zxwtest_package hello_node
查看节点 框图:
rqt_graph
我们订阅/turtle1/cmd_vel话题上的turtlesim移动的角速度和线速度信息
#include"ros/ros.h" #include"geometry_msgs/Twist.h" #include <iostream> #include <boost/asio.hpp> #include <boost/bind.hpp> #include <math.h> void chatterCallback(const geometry_msgs::Twist& msg) { ROS_INFO_STREAM(std::setprecision(2) << std::fixed << "linear.x=("<< msg.linear.x<<" msg.angular.z="<<msg.angular.z); } int main(int argc, char **argv) { ros::init(argc, argv, "listener"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("/turtle1/cmd_vel", 1000,&chatterCallback); ros::spin(); return 0; }
运行
roscore rosrun odom_tf_package listen_node rosrun turtlesim turtlesim_node rosrun turtlesim turtle_teleop_key
修改回调函数,添加向下位机发送串口数据
#include <ros/ros.h> #include "geometry_msgs/Twist.h" #include <odom_tf_package/serial_boost.h> #include <iomanip> #include <iostream> #include <boost/asio.hpp> #include <boost/bind.hpp> #include <math.h> #define MS_RADMIN 233 #define width_robot 0.31 #define BYTE0(pointer) (*((char*)(&pointer)+0)); using namespace std; using namespace boost::asio; unsigned char buf[6] = {0xff,0x00,0x00,0x00,0x00,0xfe}; void chatterCallback(const geometry_msgs::Twist& msg) { io_service iosev; //boos double vel_x = msg.linear.x / 10.0; //速度缩小十倍为0.2m/s double vel_th = msg.angular.z ; double right_vel = 0.0; double left_vel = 0.0; serial_port sp(iosev, "/dev/ttyUSB0"); sp.set_option(serial_port::baud_rate(115200)); sp.set_option(serial_port::flow_control(serial_port::flow_control::none)); sp.set_option(serial_port::parity(serial_port::parity::none)); sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one)); sp.set_option(serial_port::character_size(8)); if(vel_x == 0) { right_vel = vel_th * width_robot / 2.0; left_vel = (-1) * right_vel; } else if(vel_th == 0) { left_vel = right_vel = vel_x; } else { left_vel = vel_x - vel_th * width_robot / 2.0; right_vel = vel_x + vel_th * width_robot / 2.0; } right_vel = right_vel*MS_RADMIN; //MS_RADMIN 是m/s转换为转每分的系数,根据会自己轮子大小计算 left_vel = left_vel*MS_RADMIN; ROS_INFO("The car speed r is %2f",right_vel); ROS_INFO("The car speed l is %2f",left_vel); buf[1] = (signed char)left_vel; buf[2] = (signed char)right_vel; write(sp, buffer(buf, 6)); } int main(int argc, char **argv) { ros::init(argc, argv, "listener"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("/turtle1/cmd_vel", 1000,&chatterCallback); ros::spin(); return 0; }
发给下位机速度数据:
下位机进行串口解包。