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;
}

 

 發給下位機速度數據：

下位機進行串口解包。