ROS主题发布订阅控制真实的机器人下位机-白红宇

ROS主题发布订阅控制真实的机器人下位机

阅读量：5843 次

发布时间：2019-06-18

本文共 3110 字，大约阅读时间需要 10 分钟。

先模拟控制小乌龟

新建cmd_node.ccpp文件：

#include"ros/ros.h"#include"geometry_msgs/Twist.h"                             //包含geometry_msgs::Twist消息头文件#include 
     
      #include
      
       int main(int argc, char **argv){	ros::init(argc, argv, "cmd_node");	ros::NodeHandle n;    ros::Publisher cmd_pub = n.advertise
       
        ("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;}

编译成功产生

测试

roscorerosrun turtlesim turtlesim_noderosrun zxwtest_package hello_node

查看节点框图：

rqt_graph

我们订阅/turtle1/cmd_vel话题上的turtlesim移动的角速度和线速度信息

#include"ros/ros.h"#include"geometry_msgs/Twist.h"#include 
     
      #include 
      
       #include 
       
        #include 
        
         void chatterCallback(const geometry_msgs::Twist& msg){    ROS_INFO_STREAM(std::setprecision(2) << std::fixed << "linear.x=("<< msg.linear.x<<" msg.angular.z="<

运行

roscorerosrun odom_tf_package  listen_node  rosrun turtlesim turtlesim_noderosrun turtlesim turtle_teleop_key

修改回调函数，添加向下位机发送串口数据

#include 
     
      #include "geometry_msgs/Twist.h"#include 
      
       #include 
       
        #include 
        
         #include 
         
          #include 
          
           #include 
           
            #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;}