（二）ROS系统架构及概念 ROS Architecture and Concepts 以Kinetic为主更新 附课件PPT
第2章 ROS系统架构及概念 ROS Architecture and Concepts
PPT说明：
正文用白色，命令或代码用黄色，右下角为对应中文译著页码。
这一章需要掌握ROS文件系统，运行图级，开源社区等概念，掌握基本命令，会写ROS节点，启动文件。
属于ROS基础内容，可参考：
ROS_Kinetic_04 ROS基础内容（一）：http://blog.csdn.net/zhangrelay/article/details/51384724
ROS_Kinetic_05 ROS基础内容（二）：http://blog.csdn.net/zhangrelay/article/details/51388204
ROS_Kinetic_06 ROS基础内容（三）：http://blog.csdn.net/zhangrelay/article/details/51393800
三层：
•The Filesystem level
•The Computation Graph level
•The Community level
文件系统是功能包的内部构成，文件夹结构，以及所需核心文件等；
运行图级（计算图级）节点管理器，主题之间通信等；
开源社区主要用于资料查找。
$ sudo apt-get install tree
需要查看文件夹列表，推荐使用上面命令。
$ tree -L 1
工作空间
主要有三个文件夹构成src，devel，build，注意功能用途。
$ cmake $ make $ catkin_make $ catkin build
功能包
$ rospack find usb_cam
综合功能包
$ rosstack find ros_tutorials /home/relaybot/catkin_ws/src/ros_tutorials/ros_tutorials
消息
$ rosmsg show std_msgs/Header
理解掌握消息的类型。
服务
$ rossrv show turtlesim/Spawn
计算图级
节点
主题
服务
消息
消息记录包
节点管理器master
roscore
参数服务器
开源社区
ROS系统试用练习
ROS文件系统导航
$ rospack find turtlesim /home/relaybot/catkin_ws/src/ros_tutorials/turtlesim $ rosstack find ros_comm /opt/ros/kinetic/share/ros_comm $ rosls turtlesim CHANGELOG.rst images launch package.xml srv CMakeLists.txt include msg src tutorials $ roscd turtlesim /catkin_ws/src/ros_tutorials/turtlesim$ pwd /home/relaybot/catkin_ws/src/ros_tutorials/turtlesim
创建工作空间
To see the workspace that ROS is using, use the following command: $ echo $ROS_PACKAGE_PATH You will see output similar to the following: /opt/ros/kinetic/share:/opt/ros/kinetic/stacks The folder that we are going to create is in ~/dev/catkin_ws/src/. To add this folder, we use the following commands: $ mkdir –p ~/dev/catkin_ws/src $ cd ~/dev/catkin_ws/src $ catkin_init_workspace The next step is building the workspace. To do this, we use the following commands: $ cd ~/dev/catkin_ws $ catkin_make To finish the configuration, use the following command: $ source devel/setup.bash You should have this command at the end in your ~/.bashrc file because we used it in Chapter 1, Getting Started with ROS; otherwise, you can add it using the following command: $ echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
创建功能包与综合功能包
We will create the new package in our recently initialized workspace using the following commands: $ cd ~/dev/catkin_ws/src $ catkin_create_pkg chapter2_tutorials std_msgs roscpp The format of this command includes the name of the package and the dependencies that will have the package, in our case, std_msgs and roscpp. This is shown in the following command: catkin_create_pkg [package_name] [dependency1] ... [dependencyN]
编译功能包
$ cd ~/dev/catkin_ws/ $ catkin_make
$ catkin_make --pkg chapter2_tutorials
运行ROS节点
$ roscore
$ rosnode <param> -h $ rosnode list -h
$ rosnode list
$ rosrun turtlesim turtlesim_node
$ rosnode info /turtlesim
注意，这时的/turtle1/cmd_vel是[unknown type]。
使用主题
$ rostopic bw -h
$ rosrun turtlesim turtle_teleop_key
why? $ rosnode info /turtlesim
$ rosnode info /teleop_turtle
$ rostopic echo /turtle1/cmd_vel
此处，说明使用下面命令替代原书中命令：
$ rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '[1.0, 0.0, 0.0]' '[0.0, 0.0, 1.0]'
补充图形化：
$ rosrun rqt_publisher rqt_publisher
使用服务
$ rosservice list
$ rosservice call /clear
$ rosservice type /spawn | rossrv show $ rosservice type /spawn $ rossrv show turtlesim/Spawn
使用参数服务器
$ rosparam get /background_b $ rosparam set /background_b 50 $ rosservice call clear
创建节点
example1_a.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
int main(int argc, char **argv)
{
ros::init(argc, argv, "example1_a");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<std_msgs::String>("message", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
ss << " I am the example1_a node ";
msg.data = ss.str();
//ROS_INFO("%s", msg.data.c_str());
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}代码解释参考书39-40页。
example1_b.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
void messageCallback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "example1_b");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("message", 1000, messageCallback);
ros::spin();
return 0;
}编译节点
补充，使用gedit，当然推荐用vim。
需要修改CMakeLists.txt ，具体参考ppt或书41页。
If ROS is not running on your computer, you will have to use the following command: $ roscore You can check whether ROS is running using the rosnode list command as follows: $ rosnode list Now run both nodes in different shells: $ rosrun chapter2_tutorials example1_a $ rosrun chapter2_tutorials example1_b
创建msg和srv文件
$ rosmsg show chapter2_tutorials/chapter2_msg1
$ rossrv show chapter2_tutorials/chapter2_srv1
使用新建的srv和msg文件
example2_a.cpp
#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_srv1.h"
bool add(chapter2_tutorials::chapter2_srv1::Request &req,
chapter2_tutorials::chapter2_srv1::Response &res)
{
res.sum = req.A + req.B + req.C;
ROS_INFO("request: A=%d, B=%d C=%d", (int)req.A, (int)req.B, (int)req.C);
ROS_INFO("sending back response: [%d]", (int)res.sum);
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "add_3_ints_server");
ros::NodeHandle n;
ros::ServiceServer service = n.advertiseService("add_3_ints", add);
ROS_INFO("Ready to add 3 ints.");
ros::spin();
return 0;
}注意，#include "chapter2_tutorials/chapter2_srv1.h"，由编译系统依据srv或msg自动生成对应的.h。
example2_b.cpp
#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_srv1.h"
#include <cstdlib>
int main(int argc, char **argv)
{
ros::init(argc, argv, "add_3_ints_client");
if (argc != 4)
{
ROS_INFO("usage: add_3_ints_client A B C ");
return 1;
}
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<chapter2_tutorials::chapter2_srv1>("add_3_ints");
chapter2_tutorials::chapter2_srv1 srv;
srv.request.A = atoll(argv[1]);
srv.request.B = atoll(argv[2]);
srv.request.C = atoll(argv[3]);
if (client.call(srv))
{
ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service add_two_ints");
return 1;
}
return 0;
}Now execute the following command: $ cd ~/dev/catkin_ws $ catkin_make Execute the following command lines: $ rosrun chapter2_tutorials example2_a $ rosrun chapter2_tutorials example2_b 11 22 33
example3_a.cpp
#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_msg1.h"
#include <sstream>
int main(int argc, char **argv)
{
ros::init(argc, argv, "example3_a");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<chapter2_tutorials::chapter2_msg1>("message", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
chapter2_tutorials::chapter2_msg1 msg;
msg.A = 1;
msg.B = 2;
msg.C = 3;
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}example3_b.cpp
#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_msg1.h"
void messageCallback(const chapter2_tutorials::chapter2_msg1::ConstPtr& msg)
{
ROS_INFO("I heard: [%d] [%d] [%d]", msg->A, msg->B, msg->C);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "example3_b");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("message", 1000, messageCallback);
ros::spin();
return 0;
}补充48页：
$ rosrun chapter2_tutorials example3_a $ rosrun chapter2_tutorials example3_b
启动文件launch
一次启动多个节点，但是调试信息等不显示。
chapter2.launch
<?xml version="1.0"?> <launch> <node name ="chap2_example1_a" pkg="chapter2_tutorials" type="chap2_example1_a"/> <node name ="chap2_example1_b" pkg="chapter2_tutorials" type="chap2_example1_b"/> </launch>
$ roslaunch chapter2_tutorials chapter2.launch
动态参数
chapter2.cfg （Python）
#!/usr/bin/env python
PACKAGE = "chapter2_tutorials"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("int_param", int_t, 0, "An Integer parameter", 1, 0, 100)
gen.add("double_param", double_t, 0, "A double parameter", .1, 0, 1)
gen.add("str_param", str_t, 0, "A string parameter", "Chapter2_dynamic_reconfigure")
gen.add("bool_param", bool_t, 0, "A Boolean parameter", True)
size_enum = gen.enum([ gen.const("Low", int_t, 0, "Low is 0"),
gen.const("Medium", int_t, 1, "Medium is 1"),
gen.const("High", int_t, 2, "Hight is 2")],
"Select from the list")
gen.add("size", int_t, 0, "Select from the list", 1, 0, 3, edit_method=size_enum)
exit(gen.generate(PACKAGE, "chapter2_tutorials", "chapter2_"))example4.cpp
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <chapter2_tutorials/chapter2_Config.h>
void callback(chapter2_tutorials::chapter2_Config &config, uint32_t level) {
ROS_INFO("Reconfigure Request: %d %f %s %s %d",
config.int_param, config.double_param,
config.str_param.c_str(),
config.bool_param?"True":"False",
config.size);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "example4");
dynamic_reconfigure::Server<chapter2_tutorials::chapter2_Config> server;
dynamic_reconfigure::Server<chapter2_tutorials::chapter2_Config>::CallbackType f;
f = boost::bind(&callback, _1, _2);
server.setCallback(f);
ROS_INFO("Spinning node");
ros::spin();
return 0;
}$ roscore $ rosrun chapter2_tutorials example4 $ rosrun rqt_reconfigure rqt_reconfigure
本章课件下载：http://download.csdn.net/detail/zhangrelay/9741016
补充习题与答案：
1 启动文件
使用一个启动文件，启动小乌龟并绘制方形：
turtlesim_drawsquare.launch
<!--turtlesim drawsquare launch--> <launch> <node name="turtlesim_node1" pkg="turtlesim" type="turtlesim_node"/> <node name="turtlesim_node2" pkg="turtlesim" type="turtlesim_node"/> <node name="draw_square" pkg="turtlesim" type="draw_square"/> <node name="rqt_graph" pkg="rqt_graph" type="rqt_graph"/> </launch>
2 节点和主题
turtlesim区域覆盖（无障碍物）
grid_clean.cpp
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
#include <sstream>
using namespace std;
ros::Publisher velocity_publisher;
ros::Subscriber pose_subscriber; // to determine the position for turning the robot in an absolute orientation --> in the setDesiredOrientation fn
turtlesim::Pose turtlesim_pose;
const double x_min = 0.0;
const double y_min = 0.0;
const double x_max = 11.0;
const double y_max = 11.0;
const double PI = 3.14159265359;
void move(double speed, double distance, bool isForward);
void rotate(double angular_speed, double angle, bool cloclwise); //this will rotate the turtle at specified angle from its current angle
double degrees2radians(double angle_in_degrees);
double setDesiredOrientation(double desired_angle_radians); //this will rotate the turtle at an absolute angle, whatever its current angle is
void poseCallback(const turtlesim::Pose::ConstPtr & pose_message); //Callback fn everytime the turtle pose msg is published over the /turtle1/pose topic.
void moveGoal(turtlesim::Pose goal_pose, double distance_tolerance); //this will move robot to goal
double getDistance(double x1, double y1, double x2, double y2);
void gridClean();
int main(int argc, char **argv)
{
// Initiate new ROS node named "talker"
ros::init(argc, argv, "turtlesim_cleaner");
ros::NodeHandle n;
double speed, angular_speed;
double distance, angle;
bool isForward, clockwise;
velocity_publisher = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
pose_subscriber = n.subscribe("/turtle1/pose", 10, poseCallback); //call poseCallback everytime the turtle pose msg is published over the /turtle1/pose topic.
ros::Rate loop_rate(0.5);
// /turtle1/cmd_vel is the Topic name
// /geometry_msgs::Twist is the msg type
ROS_INFO("\n\n\n ********START TESTING*********\n");
/*********This is to move and rotate the robot as the user.**************
cout<<"enter speed: ";
cin>>speed;
cout<<"enter distance: ";
cin>>distance;
cout<<"forward?: ";
cin>>isForward;
move(speed, distance, isForward);
cout<<"enter angular velocity: ";
cin>>angular_speed;
cout<<"enter angle: ";
cin>>angle;
cout<<"Clockwise?: ";
cin>>clockwise;
rotate(degrees2radians(angular_speed), degrees2radians(angle), clockwise);
*/
/**************This is to change the Absolute Orientation***************
setDesiredOrientation(degrees2radians(120));
ros::Rate loop_rate(0.5);
loop_rate.sleep();
setDesiredOrientation(degrees2radians(-60));
loop_rate.sleep();
setDesiredOrientation(degrees2radians(0));
*/
/****************This is to move the robot to a goal position*************
turtlesim::Pose goal_pose;
goal_pose.x = 1;
goal_pose.y = 1;
goal_pose.theta = 0;
moveGoal(goal_pose, 0.01);
loop_rate.sleep();
*/
gridClean();
ros::spin();
return 0;
}
/**
* makes the robot move with a certain linear velocity for a
* certain distance in a forward or backward straight direction.
*/
void move(double speed, double distance, bool isForward)
{
geometry_msgs::Twist vel_msg;
//set a random linear velocity in the x-axis
if (isForward)
vel_msg.linear.x =abs(speed);
else
vel_msg.linear.x =-abs(speed);
vel_msg.linear.y =0;
vel_msg.linear.z =0;
//set a random angular velocity in the y-axis
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z =0;
double t0 = ros::Time::now().toSec();
double current_distance = 0.0;
ros::Rate loop_rate(100);
do{
velocity_publisher.publish(vel_msg);
double t1 = ros::Time::now().toSec();
current_distance = speed * (t1-t0);
ros::spinOnce();
loop_rate.sleep();
//cout<<(t1-t0)<<", "<<current_distance <<", "<<distance<<endl;
}while(current_distance<distance);
vel_msg.linear.x =0;
velocity_publisher.publish(vel_msg);
}
/**
* makes the robot turn with a certain angular velocity, for
* a certain distance in either clockwise or counter-clockwise direction
*/
void rotate (double angular_speed, double relative_angle, bool clockwise)
{
geometry_msgs::Twist vel_msg;
//set a random linear velocity in the x-axis
vel_msg.linear.x =0;
vel_msg.linear.y =0;
vel_msg.linear.z =0;
//set a random angular velocity in the y-axis
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
if (clockwise)
vel_msg.angular.z =-abs(angular_speed);
else
vel_msg.angular.z =abs(angular_speed);
double t0 = ros::Time::now().toSec();
double current_angle = 0.0;
ros::Rate loop_rate(1000);
do{
velocity_publisher.publish(vel_msg);
double t1 = ros::Time::now().toSec();
current_angle = angular_speed * (t1-t0);
ros::spinOnce();
loop_rate.sleep();
//cout<<(t1-t0)<<", "<<current_angle <<", "<<relative_angle<<endl;
}while(current_angle<relative_angle);
vel_msg.angular.z =0;
velocity_publisher.publish(vel_msg);
}
/**
* converts angles from degree to radians
*/
double degrees2radians(double angle_in_degrees)
{
return angle_in_degrees *PI /180.0;
}
/**
* turns the robot to a desried absolute angle
*/
double setDesiredOrientation(double desired_angle_radians)
{
double relative_angle_radians = desired_angle_radians - turtlesim_pose.theta;
//if we want to turn at a perticular orientation, we subtract the current orientation from it
bool clockwise = ((relative_angle_radians<0)?true:false);
//cout<<desired_angle_radians <<","<<turtlesim_pose.theta<<","<<relative_angle_radians<<","<<clockwise<<endl;
rotate (abs(relative_angle_radians), abs(relative_angle_radians), clockwise);
}
/**
* A callback function to update the pose of the robot
*/
void poseCallback(const turtlesim::Pose::ConstPtr & pose_message)
{
turtlesim_pose.x=pose_message->x;
turtlesim_pose.y=pose_message->y;
turtlesim_pose.theta=pose_message->theta;
}
/*
* A proportional controller to make the robot moves towards a goal pose
*/
void moveGoal(turtlesim::Pose goal_pose, double distance_tolerance)
{
//We implement a Proportional Controller. We need to go from (x,y) to (x',y'). Then, linear velocity v' = K ((x'-x)^2 + (y'-y)^2)^0.5 where K is the constant and ((x'-x)^2 + (y'-y)^2)^0.5 is the Euclidian distance. The steering angle theta = tan^-1(y'-y)/(x'-x) is the angle between these 2 points.
geometry_msgs::Twist vel_msg;
ros::Rate loop_rate(10);
do{
//linear velocity
vel_msg.linear.x = 1.5*getDistance(turtlesim_pose.x, turtlesim_pose.y, goal_pose.x, goal_pose.y);
vel_msg.linear.y = 0;
vel_msg.linear.z = 0;
//angular velocity
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z = 4*(atan2(goal_pose.y - turtlesim_pose.y, goal_pose.x - turtlesim_pose.x)-turtlesim_pose.theta);
velocity_publisher.publish(vel_msg);
ros::spinOnce();
loop_rate.sleep();
}while(getDistance(turtlesim_pose.x, turtlesim_pose.y, goal_pose.x, goal_pose.y)>distance_tolerance);
cout<<"end move goal"<<endl;
vel_msg.linear.x = 0;
vel_msg.angular.z = 0;
velocity_publisher.publish(vel_msg);
}
/*
* get the euclidian distance between two points
*/
double getDistance(double x1, double y1, double x2, double y2)
{
return sqrt(pow((x2-x1),2) + pow((y2-y1),2));
}
/*
* the cleaning appication function. returns true when completed.
*/
void gridClean()
{
ros::Rate loop(0.5);
turtlesim::Pose goal_pose;
goal_pose.x = 1;
goal_pose.y = 1;
goal_pose.theta = 0;
moveGoal(goal_pose, 0.01);
loop.sleep();
setDesiredOrientation(0);
loop.sleep();
move(2,9, true);
loop.sleep();
rotate(degrees2radians(10), degrees2radians(90), false);
loop.sleep();
move(2,9,true);
rotate(degrees2radians(10), degrees2radians(90), false);
loop.sleep();
move(2,1,true);
rotate(degrees2radians(10), degrees2radians(90), false);
loop.sleep();
move(2,9, true);
rotate(degrees2radians(30), degrees2radians(90), true);
loop.sleep();
move(2,1,true);
rotate(degrees2radians(30), degrees2radians(90), true);
loop.sleep();
move(2,9, true);
//double distance = getDistance(turtlesim_pose.x, turtlesim_pose.y, x_max
}spiral_clean.cpp
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
#include <sstream>
using namespace std;
ros::Publisher velocity_publisher;
ros::Subscriber pose_subscriber; // to determine the position for turning the robot in an absolute orientation --> in the setDesiredOrientation fn
turtlesim::Pose turtlesim_pose;
const double x_min = 0.0;
const double y_min = 0.0;
const double x_max = 11.0;
const double y_max = 11.0;
const double PI = 3.14159265359;
void move(double speed, double distance, bool isForward);
void rotate(double angular_speed, double angle, bool cloclwise); //this will rotate the turtle at specified angle from its current angle
double degrees2radians(double angle_in_degrees);
double setDesiredOrientation(double desired_angle_radians); //this will rotate the turtle at an absolute angle, whatever its current angle is
void poseCallback(const turtlesim::Pose::ConstPtr & pose_message); //Callback fn everytime the turtle pose msg is published over the /turtle1/pose topic.
void moveGoal(turtlesim::Pose goal_pose, double distance_tolerance); //this will move robot to goal
double getDistance(double x1, double y1, double x2, double y2);
void gridClean();
void spiralClean();
int main(int argc, char **argv)
{
// Initiate new ROS node named "talker"
ros::init(argc, argv, "turtlesim_cleaner");
ros::NodeHandle n;
double speed, angular_speed;
double distance, angle;
bool isForward, clockwise;
velocity_publisher = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
pose_subscriber = n.subscribe("/turtle1/pose", 10, poseCallback); //call poseCallback everytime the turtle pose msg is published over the /turtle1/pose topic.
ros::Rate loop_rate(0.5);
// /turtle1/cmd_vel is the Topic name
// /geometry_msgs::Twist is the msg type
ROS_INFO("\n\n\n ********START TESTING*********\n");
/*********This is to move and rotate the robot as the user.**************
cout<<"enter speed: ";
cin>>speed;
cout<<"enter distance: ";
cin>>distance;
cout<<"forward?: ";
cin>>isForward;
move(speed, distance, isForward);
cout<<"enter angular velocity: ";
cin>>angular_speed;
cout<<"enter angle: ";
cin>>angle;
cout<<"Clockwise?: ";
cin>>clockwise;
rotate(degrees2radians(angular_speed), degrees2radians(angle), clockwise);
*/
/**************This is to change the Absolute Orientation***************
setDesiredOrientation(degrees2radians(120));
ros::Rate loop_rate(0.5);
loop_rate.sleep();
setDesiredOrientation(degrees2radians(-60));
loop_rate.sleep();
setDesiredOrientation(degrees2radians(0));
*/
/****************This is to move the robot to a goal position*************
turtlesim::Pose goal_pose;
goal_pose.x = 1;
goal_pose.y = 1;
goal_pose.theta = 0;
moveGoal(goal_pose, 0.01);
loop_rate.sleep();
*/
//gridClean(); //for the grid clean
spiralClean();
ros::spin();
return 0;
}
/**
* makes the robot move with a certain linear velocity for a
* certain distance in a forward or backward straight direction.
*/
void move(double speed, double distance, bool isForward)
{
geometry_msgs::Twist vel_msg;
//set a random linear velocity in the x-axis
if (isForward)
vel_msg.linear.x =abs(speed);
else
vel_msg.linear.x =-abs(speed);
vel_msg.linear.y =0;
vel_msg.linear.z =0;
//set a random angular velocity in the y-axis
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z =0;
double t0 = ros::Time::now().toSec();
double current_distance = 0.0;
ros::Rate loop_rate(100);
do{
velocity_publisher.publish(vel_msg);
double t1 = ros::Time::now().toSec();
current_distance = speed * (t1-t0);
ros::spinOnce();
loop_rate.sleep();
//cout<<(t1-t0)<<", "<<current_distance <<", "<<distance<<endl;
}while(current_distance<distance);
vel_msg.linear.x =0;
velocity_publisher.publish(vel_msg);
}
/**
* makes the robot turn with a certain angular velocity, for
* a certain distance in either clockwise or counter-clockwise direction
*/
void rotate (double angular_speed, double relative_angle, bool clockwise)
{
geometry_msgs::Twist vel_msg;
//set a random linear velocity in the x-axis
vel_msg.linear.x =0;
vel_msg.linear.y =0;
vel_msg.linear.z =0;
//set a random angular velocity in the y-axis
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
if (clockwise)
vel_msg.angular.z =-abs(angular_speed);
else
vel_msg.angular.z =abs(angular_speed);
double t0 = ros::Time::now().toSec();
double current_angle = 0.0;
ros::Rate loop_rate(1000);
do{
velocity_publisher.publish(vel_msg);
double t1 = ros::Time::now().toSec();
current_angle = angular_speed * (t1-t0);
ros::spinOnce();
loop_rate.sleep();
//cout<<(t1-t0)<<", "<<current_angle <<", "<<relative_angle<<endl;
}while(current_angle<relative_angle);
vel_msg.angular.z =0;
velocity_publisher.publish(vel_msg);
}
/**
* converts angles from degree to radians
*/
double degrees2radians(double angle_in_degrees)
{
return angle_in_degrees *PI /180.0;
}
/**
* turns the robot to a desried absolute angle
*/
double setDesiredOrientation(double desired_angle_radians)
{
double relative_angle_radians = desired_angle_radians - turtlesim_pose.theta;
//if we want to turn at a perticular orientation, we subtract the current orientation from it
bool clockwise = ((relative_angle_radians<0)?true:false);
//cout<<desired_angle_radians <<","<<turtlesim_pose.theta<<","<<relative_angle_radians<<","<<clockwise<<endl;
rotate (abs(relative_angle_radians), abs(relative_angle_radians), clockwise);
}
/**
* A callback function to update the pose of the robot
*/
void poseCallback(const turtlesim::Pose::ConstPtr & pose_message)
{
turtlesim_pose.x=pose_message->x;
turtlesim_pose.y=pose_message->y;
turtlesim_pose.theta=pose_message->theta;
}
/*
* A proportional controller to make the robot moves towards a goal pose
*/
void moveGoal(turtlesim::Pose goal_pose, double distance_tolerance)
{
//We implement a Proportional Controller. We need to go from (x,y) to (x',y'). Then, linear velocity v' = K ((x'-x)^2 + (y'-y)^2)^0.5 where K is the constant and ((x'-x)^2 + (y'-y)^2)^0.5 is the Euclidian distance. The steering angle theta = tan^-1(y'-y)/(x'-x) is the angle between these 2 points.
geometry_msgs::Twist vel_msg;
ros::Rate loop_rate(10);
do{
//linear velocity
vel_msg.linear.x = 1.5*getDistance(turtlesim_pose.x, turtlesim_pose.y, goal_pose.x, goal_pose.y);
vel_msg.linear.y = 0;
vel_msg.linear.z = 0;
//angular velocity
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z = 4*(atan2(goal_pose.y - turtlesim_pose.y, goal_pose.x - turtlesim_pose.x)-turtlesim_pose.theta);
velocity_publisher.publish(vel_msg);
ros::spinOnce();
loop_rate.sleep();
}while(getDistance(turtlesim_pose.x, turtlesim_pose.y, goal_pose.x, goal_pose.y)>distance_tolerance);
cout<<"end move goal"<<endl;
vel_msg.linear.x = 0;
vel_msg.angular.z = 0;
velocity_publisher.publish(vel_msg);
}
/*
* get the euclidian distance between two points
*/
double getDistance(double x1, double y1, double x2, double y2)
{
return sqrt(pow((x2-x1),2) + pow((y2-y1),2));
}
/*
* the cleaning appication function. returns true when completed.
*/
void gridClean()
{
ros::Rate loop(0.5);
turtlesim::Pose goal_pose;
goal_pose.x = 1;
goal_pose.y = 1;
goal_pose.theta = 0;
moveGoal(goal_pose, 0.01);
loop.sleep();
setDesiredOrientation(0);
loop.sleep();
move(2,9, true);
loop.sleep();
rotate(degrees2radians(10), degrees2radians(90), false);
loop.sleep();
move(2,9,true);
rotate(degrees2radians(10), degrees2radians(90), false);
loop.sleep();
move(2,1,true);
rotate(degrees2radians(10), degrees2radians(90), false);
loop.sleep();
move(2,9, true);
rotate(degrees2radians(30), degrees2radians(90), true);
loop.sleep();
move(2,1,true);
rotate(degrees2radians(30), degrees2radians(90), true);
loop.sleep();
move(2,9, true);
//double distance = getDistance(turtlesim_pose.x, turtlesim_pose.y, x_max
}
void spiralClean()
{
geometry_msgs::Twist vel_msg;
double count = 0;
double constant_speed = 4;
double vk = 1;
double wk = 2;
double rk = 0.5;
ros::Rate loop(1);
do{
rk = rk + 0.5;
vel_msg.linear.x = rk;
vel_msg.linear.y = 0;
vel_msg.linear.z = 0;
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
vel_msg.angular.z = constant_speed;
cout<<"vel_msg.linear.x = "<<vel_msg.linear.x<<endl;
cout<<"vel_msg.angular.z = "<<vel_msg.angular.z<<endl;
velocity_publisher.publish(vel_msg);
ros::spinOnce();
loop.sleep();
cout<<rk<<" , "<<vk <<" , "<<wk<<endl;
}while((turtlesim_pose.x<10.5)&&(turtlesim_pose.y<10.5));
vel_msg.linear.x = 0;
velocity_publisher.publish(vel_msg);
}3 一个节点发布小乌龟位置姿态信息，另一个节点订阅并移动小乌龟到指定位姿。(参考示例Python)
move.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
def move():
# Starts a new node
rospy.init_node('robot_cleaner', anonymous=True)
velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
vel_msg = Twist()
#Receiveing the user's input
print("Let's move your robot")
speed = input("Input your speed:")
distance = input("Type your distance:")
isForward = input("Foward?: ")
#Checking if the movement is forward or backwards
if(isForward):
vel_msg.linear.x = abs(speed)
else:
vel_msg.linear.x = -abs(speed)
#Since we are moving just in x-axis
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 0
while not rospy.is_shutdown():
#Setting the current time for distance calculus
t0 = float(rospy.Time.now().to_sec())
current_distance = 0
#Loop to move the turtle in an specified distance
while(current_distance < distance):
#Publish the velocity
velocity_publisher.publish(vel_msg)
#Takes actual time to velocity calculus
t1=float(rospy.Time.now().to_sec())
#Calculates distancePoseStamped
current_distance= speed*(t1-t0)
#After the loop, stops the robot
vel_msg.linear.x = 0
#Force the robot to stop
velocity_publisher.publish(vel_msg)
if __name__ == '__main__':
try:
#Testing our function
move()
except rospy.ROSInterruptException: passrotate.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
PI = 3.1415926535897
def rotate():
#Starts a new node
rospy.init_node('robot_cleaner', anonymous=True)
velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
vel_msg = Twist()
# Receiveing the user's input
print("Let's rotate your robot")
speed = input("Input your speed (degrees/sec):")
angle = input("Type your distance (degrees):")
clockwise = input("Clowkise?: ") #True or false
#Converting from angles to radians
angular_speed = speed*2*PI/360
relative_angle = angle*2*PI/360
#We wont use linear components
vel_msg.linear.x=0
vel_msg.linear.y=0
vel_msg.linear.z=0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
# Checking if our movement is CW or CCW
if clockwise:
vel_msg.angular.z = -abs(angular_speed)
else:
vel_msg.angular.z = abs(angular_speed)
# Setting the current time for distance calculus
t0 = rospy.Time.now().to_sec()
current_angle = 0
while(current_angle < relative_angle):
velocity_publisher.publish(vel_msg)
t1 = rospy.Time.now().to_sec()
current_angle = angular_speed*(t1-t0)
#Forcing our robot to stop
vel_msg.angular.z = 0
velocity_publisher.publish(vel_msg)
rospy.spin()
if __name__ == '__main__':
try:
# Testing our function
rotate()
except rospy.ROSInterruptException:passgotogoal.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import pow,atan2,sqrt
class turtlebot():
def __init__(self):
#Creating our node,publisher and subscriber
rospy.init_node('turtlebot_controller', anonymous=True)
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, self.callback)
self.pose = Pose()
self.rate = rospy.Rate(10)
#Callback function implementing the pose value received
def callback(self, data):
self.pose = data
self.pose.x = round(self.pose.x, 4)
self.pose.y = round(self.pose.y, 4)
def get_distance(self, goal_x, goal_y):
distance = sqrt(pow((goal_x - self.pose.x), 2) + pow((goal_y - self.pose.y), 2))
return distance
def move2goal(self):
goal_pose = Pose()
goal_pose.x = input("Set your x goal:")
goal_pose.y = input("Set your y goal:")
distance_tolerance = input("Set your tolerance:")
vel_msg = Twist()
while sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2)) >= distance_tolerance:
#Porportional Controller
#linear velocity in the x-axis:
vel_msg.linear.x = 1.5 * sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2))
vel_msg.linear.y = 0
vel_msg.linear.z = 0
#angular velocity in the z-axis:
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 4 * (atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.theta)
#Publishing our vel_msg
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
#Stopping our robot after the movement is over
vel_msg.linear.x = 0
vel_msg.angular.z =0
self.velocity_publisher.publish(vel_msg)
rospy.spin()
if __name__ == '__main__':
try:
#Testing our function
x = turtlebot()
x.move2goal()
except rospy.ROSInterruptException: pass-End-
