第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

参数服务器

开源社区

资料宝库!http://wiki.ros.org/cn/

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: pass

rotate.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:pass

gotogoal.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-

(二)ROS系统架构及概念 ROS Architecture and Concepts 以Kinetic为主更新 附课件PPT的更多相关文章

  1. (一)ROS系统入门 Getting Started with ROS 以Kinetic为主更新 附课件PPT

    ROS机器人程序设计(原书第2版)补充资料 教案1 ROS Kinetic系统入门 ROS Kinetic在Ubuntu 16.04.01 安装可参考:http://blog.csdn.net/zha ...

  2. ROS机器人程序设计(原书第2版)补充资料 (贰) 第二章 ROS系统架构及概念

    ROS机器人程序设计(原书第2版)补充资料 (贰) 第二章 ROS系统架构及概念 书中,大部分出现hydro的地方,直接替换为indigo或jade或kinetic,即可在对应版本中使用. 由于工作事 ...

  3. ROS学习笔记二:ROS系统架构及概念

    本文主要是了解ROS的系统架构.文件夹结构以及工作所需的核心文件. ROS系统架构主要被设计和划分为三部分,每一部分代表一个层级的概念:文件系统级(The filesystem level).计算图级 ...

  4. Windows Internals学习笔记(二)系统架构

    参考资料: 1. <Windows Internals> 2. http://bestcbooks.com 3. Windows Drive Kit 4. Microsoft Window ...

  5. 系统架构:Web应用架构的新趋势---前端和后端分离的一点想法

    最近研究servlet,看书时候书里讲到了c/s架构到b/s架构的演变,讲servlet的书都很老了,现在的b/s架构已经不是几年前的b/s架构,其实b/s架构就是web应用开发,对于这样的架构我们现 ...

  6. 系统架构-设计模式(适配器、观察者、代理、抽象工厂等)及架构模式(C/S、B/S、分布式、SOA、SaaS)(干货)

    博客园首页是需要分享干货的地方,今天早上写的<HRMS(人力资源管理系统)-从单机应用到SaaS应用-系统介绍>内容下架了,所以我就按照相关规定,只分享干货,我把之前写完的内容整理发布上来 ...

  7. MYSQL复习笔记1-物理文件和系统架构

    date:20140101auth:Jin 一.物理组成(一) 日志文件参考:http://dev.mysql.com/doc/refman/5.1/en/server-logs.html1.错误日志 ...

  8. stm32.cube(一)——系统架构及目录结构

    一.前言 Arm的应用场景往往比51单片机复杂得多,如果一个高级应用的开发需要连底层的结构性代码都要重构,那么在成本和研发周期上就会面临巨大的风险.为了简化编码过程,芯片厂商经常会提供一些板卡级支持的 ...

  9. vivo 全球商城:优惠券系统架构设计与实践

    一.业务背景 优惠券是电商常见的营销手段,具有灵活的特点,既可以作为促销活动的载体,也是重要的引流入口.优惠券系统是vivo商城营销模块中一个重要组成部分,早在15年vivo商城还是单体应用时,优惠券 ...

随机推荐

  1. POJ- 1094 Sorting It All Out---拓扑排序是否唯一的判断

    题目链接: https://vjudge.net/problem/POJ-1094 题目大意: 该题题意明确,就是给定一组字母的大小关系判断他们是否能组成唯一的拓扑序列.是典型的拓扑排序,但输出格式上 ...

  2. Menu-菜单组件

    #menu菜单组件 from tkinter import * master=Tk() def callback(): print('你好咯!!') m = Menu(master) m.add_co ...

  3. 使用poi导出数据到excel

    一.首先是导入poi所需要的jar包,我是用的是maven,添加jar包依赖 <dependency> <groupId>org.apache.poi</groupId& ...

  4. [Micropython]TPYBoard v10x拼插编程实验 点亮心形点阵

    一.什么是TPYBoard开发板 TPYBoard是以遵照MIT许可的MicroPython为基础,由TurnipSmart公司制作的一款MicroPython开发板,它基于STM32F405单片机, ...

  5. 技巧:如何提升Oracle用户密码的安全性

    环境:Oracle 11.2.0.4 客户需求:主要背景是数据库中有很多业务用户名,且由于部分用户缺乏安全意识,甚至直接将自己的密码设置为和用户名一样,目前客户期望密码设置不要过于简单,最起码别和用户 ...

  6. C#之Winform跨线程访问控件

    C#中禁止跨线程直接访问控件,InvokeRequired是为了解决这个问题而产生的,当一个控件的InvokeRequired属性值为真时,说明有一个创建它以外的线程想访问它.此时它将会在内部调用ne ...

  7. Thread源码剖析

    前言 昨天已经写了: 多线程三分钟就可以入个门了! 如果没看的同学建议先去阅读一遍哦~ 在写文章之前通读了一遍<Java 核心技术 卷一>的并发章节和<Java并发编程实战>前 ...

  8. css 的一些知识点的整理

    css的一些标签整理   background-attachment: scroll;背景图可滚动 background-attachment: fixed; 固定背景的位置,不随着滚动条移动而移动 ...

  9. [APIO 2012]派遣

    Description 在一个忍者的帮派里,一些忍者们被选中派遣给顾客,然后依据自己的工作获取报偿. 在这个帮派里,有一名忍者被称之为Master.除了Master以外,每名忍者都有且仅有一个上级.为 ...

  10. Educational Codeforces Round 19

    A. k-Factorization 题目大意:给一个数n,求k个大于1的数,乘积为n.(n<=100,000,k<=20) 思路:分解质因数呗 #include<cstdio> ...