先模拟控制小乌龟
新建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
修改回调函数,添加向下位机发送串口数据
原文:http://www.cnblogs.com/CZM-/p/5928277.html