一、版本要求
工作站设计使用的系统版本：Ubuntu16.04
工作站设计使用的Ros版本：kinetic版本
桌面完全安装：ROS，rqt，rviz，机器人通用库，2D / 3D模拟器，导航和2D / 3D感知
二、创建ROS的工作空间
catkin_make命令是使用catkin_ws的一种方便的工具，在工作空间里第一次运行它，它将创建CMakeLists.txt来与”src”目录相链接，此时当前目录下会存在”build”和“devel”目录，在创建的环境上覆盖工作空间
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make
启动devel/setup.bash文件
source devel/setup.bash
三、设计过程
源码包介绍：catkin_ws/src/simulation、catkin_ws/src/simulation_launch、catkin_ws/src/ move_base_sim
3.1、simulation目录
3.1.1 小车的位置与静态地图中传感器的数据
autolabor_faker.cpp 这个包是获得autolabor小车的位置，以及静态地图中传感器的数据。发布小车的里程计的相关消息。
autolabor_faker.cpp代码如下：
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TwistStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <cmath>
float vx = 0.;
float vy = 0.;
float vth = 0.;
double max_x_accel;
double max_theta_accel;
double cmd_timeout;
ros::Time last_callback;
static float joint2_stat_last = 0.;
inline void getValidVelocity( float& v, const float tmp_v, const double max_delta_v )
{
double sign = tmp_v > v ? 1.0 : -1.0;
v = v + sign * max_delta_v;
}
void cmdCallback(geometry_msgs::Twist::ConstPtr cmd)
{
static ros::Time last_time = ros::Time::now();
ros::Time current_time = ros::Time::now();
double dt = ( current_time - last_time ).toSec();
float max_delta_vx = max_x_accel * dt;
float max_delta_vth = max_theta_accel * dt;
ROS_INFO("I heard twist message, vx: %lf, vy: %lf, vtheta: %lf", cmd->linear.x, cmd->linear.y, cmd->angular.z);
float tmp_vx = cmd->linear.x;
float tmp_vth = cmd->angular.z;
float d_vx = vx - tmp_vx;
float d_vth = vth - tmp_vth;
vy = 0.;
//if ( fabs(d_vx) > max_delta_vx )
//{
//ROS_WARN("accelerat ax too large, reset velocity ");
//getValidVelocity( vx, tmp_vx, max_delta_vx );
//} else {
vx = tmp_vx;
//}
//if ( fabs(d_vth) > max_delta_vth )
//{
//ROS_WARN("accelerat atheta too large, reset velocity ");
//getValidVelocity( vth, tmp_vth, max_delta_vth );
//} else {
vth = tmp_vth;
//}
last_callback = last_time = current_time;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "autolabor_fake");
ros::NodeHandle nh("autolabor");
//ros::NodeHandle nh;
ros::Publisher odom_pub = nh.advertise< nav_msgs::Odometry >("odom", 10);
tf::TransformBroadcaster odom_broadcaster;
ros::Subscriber cmd_sub = nh.subscribe< geometry_msgs::Twist >("cmd_vel", 10, cmdCallback);
nh.param( "max_x_accel", max_x_accel, 1.0 );
nh.param( "max_theta_accel", max_theta_accel, 1.0 );
nh.param( "cmd_timeout", cmd_timeout, 0.2 );
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
float x = 0.;
float y = 0.;
float th = 0.;
ros::Rate rate(10.0);
while(nh.ok())
{
ros::spinOnce();
current_time = ros::Time::now();
if ( (current_time - last_callback).toSec() > cmd_timeout )
{
ROS_WARN( "subscriber timeout, set all velocity to Zero" );
vx = 0.; vy = 0.; vth = 0.;
}
float dt = (current_time - last_time).toSec();
float delta_x = vx*cos(th)*dt - vy*sin(th)*dt;
float delta_y = vx*sin(th)*dt + vy*cos(th)*dt;
float delta_th = vth*dt;
x += delta_x;
y += delta_y;
th += delta_th;
// setuo tf frame
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(th);
odom_trans.transform.rotation = quat;
// broadcast tf frame
odom_broadcaster.sendTransform(odom_trans);
//set up odom frame
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "base_link";
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = quat;
odom.child_frame_id = "odom";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;
// publish odom frame
odom_pub.publish(odom);
last_time = current_time;
rate.sleep();
}
}
3.1.2小车的3D模型
通过在simulation中创建了一autolabor_description的包，该命令的最后一个参数是引入urdf库。
在新建的包中创建 urdf 和 launch 文件夹用于存储 *.urdf 和 *.launch 文件。可以在catkin_ws/src/目录下看到：
3.1.2.1绘制机器人小车的3D模据型
使用使用3D制图软件绘制机器人小车的3D模型，在autolabor_description包的目录下，新建meshes文件夹，将绘制的3D模型复制到在路径下
3.1.2.2创建urdf文件夹
在urdf文件夹下创建文件 autolabor_description.urdf文件，并将以下代码放在 autolabor_description.urdf文件中。
<robot name="autolabor_description">
<link name="base_link">//对应模型的一个模块，可以通过标签joint让子模块与base_link进行关联;
<inertial>
<origin
xyz="0. 0. 0."
rpy="0. 0. 0." />
<mass
value="0.251988675650349" />
<inertia
ixx="0.000595579869264794"
ixy="5.99238175321912E-08"
ixz="-1.98242615307314E-08"
iyy="0.00102462329604677"
iyz="-1.73115625503396E-05"
izz="0.00060561972360446" />
</inertial>//定义惯性；
<visual>
<origin
xyz="0. 0. 0.05"
rpy="1.57 0. 1.57" />//用来描述模块的位置；
<geometry>
<mesh
filename="package://autolabor_description/meshes/base_link.stl" />
</geometry>//定义该link的几何模型，包含该几何模型的尺寸，单位：米；
<material
name="">//定义颜色和透明度（RGBA），取值区间 [0,1] ；
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>//描述一个link的外观，大小，颜色，材质纹理等;
</link>
</robot>
3.1.2.3创建launch命令文件
*.launch文件的作用是:同时启动多个节点,使用roslaunch命令运行.launch文件中指定的节点。在launch文件夹中创建文件display.launch，并编辑，display.launch代码如下：
<launch>
<arg name="model" />
<arg name="gui" default="false" />
<param name="robot_description" textfile="$(find autolabor_description)/urdf/autolabor_description.urdf" />
<param name="use_gui" value="$(arg gui)" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find autolabor_description)/urdf.rviz" />
</launch>
第一个输入参数 model 就是要启动的urdf文件路径。
第二个输入参数 gui 指定是否启用关节转动控制面板窗口。
两个参数表示：分别描述要启动的模型描述文件（urdf）和关节转到控制窗口（gui，对应各个joint）。
三个节点：分别用于发送joint的信息，robot的控制信息，和rviz的启动。至此，模型搭建完毕，启动模型进行测试。
到此，机器人小车便已建立完成，启动小车
cd ~/catkin_ws/
source devel/setup.bash
catkin_make
roslaunch autolabor_description display.launch
3.1.3 单线激光雷达的模拟数据
lidar_simulation提供单线激光雷达的模拟数据，这里主要包括雷达模拟数据、雷达模拟障碍物的数据，雷达模拟静态地图的数据。
3.1.3.1 雷达模拟数据
lidar_simulation.cpp代码：
#include "iostream"
#include "nav_msgs/GetMap.h"
#include "lidar_simulation/lidar_simulation.h"
namespace autolabor_simulation {
LidarSimulation::LidarSimulation(){
ros::NodeHandle private_node("~");
private_node.param("min_angle", min_angle_, -M_PI);
private_node.param("max_angle", max_angle_, M_PI);
private_node.param("min_distance", min_dis_, 0.15);
private_node.param("max_distance", max_dis_, 6.00);
private_node.param("size", point_size_, 400);
private_node.param("rate", rate_, 10);
private_node.param("global_frame", global_frame_, string("map"));
private_node.param("lidar_frame", lidar_frame_, string("lidar"));
step_ = (max_angle_ - min_angle_) / (point_size_ - 1);
}
LidarSimulation::LidarSimulation(double min_angle, double max_angle, double min_dis, double max_dis, int point_size, int rate):
min_angle_(min_angle), max_angle_(max_angle),
min_dis_(min_dis), max_dis_(max_dis),
point_size_(point_size),rate_(rate)
{
step_ = (max_angle_ - min_angle_) / (point_size_ - 1);
}
LidarSimulation::~LidarSimulation(){}
void LidarSimulation::updateMap(ros::ServiceClient &client){
ros::Rate fetch_rate(1);
nav_msgs::GetMap message;
while (true){
if (client.call(message)){
map_.initMap(message.response.map);
ROS_INFO("Get map! size : %d x %d", message.response.map.info.height, message.response.map.info.width);
break;
}else{
ROS_INFO("Waiting for the Map!");
}
fetch_rate.sleep();
}
}
void LidarSimulation::getPose(tf::StampedTransform& transform, double& start_angle, double& reverse){
double roll, pitch, yaw;
tf_.lookupTransform(global_frame_, lidar_frame_, ros::Time(), transform);
transform.getBasis().getRPY(roll, pitch, yaw);
if (pow(roll,2) + pow(pitch,2) > esp){
start_angle = yaw + max_angle_;
reverse = -1.0;
}else{
start_angle = yaw + min_angle_;
reverse = 1.0;
}
}
void LidarSimulation::initLaserScan(sensor_msgs::LaserScan &laser_scan){
laser_scan.header.frame_id = lidar_frame_;
laser_scan.angle_min = min_angle_;
laser_scan.angle_max = max_angle_;
laser_scan.angle_increment = step_;
laser_scan.time_increment = 1 / point_size_ / rate_;
laser_scan.range_min = min_dis_;
laser_scan.range_max = max_dis_;
}
void LidarSimulation::getFrame(sensor_msgs::LaserScan &laser_scan){
tf::StampedTransform transform;
double start_angle, reverse, angle;
float start_wx, start_wy, end_wx, end_wy, dis;
laser_scan.header.stamp = ros::Time::now();
getPose(transform, start_angle, reverse);
start_wx = (float)transform.getOrigin().getX();
start_wy = (float)transform.getOrigin().getY();
laser_scan.ranges.clear();
for (int i=0; i<point_size_; i++){
angle = start_angle + i * reverse * step_;
map_.distance(start_wx, start_wy, angle, end_wx, end_wy);
dis = sqrt(pow(end_wx - start_wx, 2) + pow(end_wy - start_wy, 2));
if (dis <= max_dis_ && dis >= min_dis_){
laser_scan.ranges.push_back(dis);
}else{
laser_scan.ranges.push_back(numeric_limits<float>::infinity());
}
}
}
bool LidarSimulation::obstacleHandleServer(lidar_simulation::Obstacle::Request &req, lidar_simulation::Obstacle::Response &res){
if (req.type == req.TRANSFORM){
map_.transformObstacle(req.obstacle_id, req.transform[0], req.transform[1], req.transform[2]);
}else if (req.type == req.NEW){
map_.insertObstacle(req.obstacle_id, req.vertex, req.transform[0], req.transform[1], req.transform[2]);
}else if (req.type == req.DELETE){
map_.deleteObstacle(req.obstacle_id);
}else{
return false;
}
map_.updateObstacleData();
return true;
}
void LidarSimulation::pubLaserCallback(const ros::TimerEvent& event){
sensor_msgs::LaserScan laserscan;
initLaserScan(laserscan);
getFrame(laserscan);
pub_.publish(laserscan);
}
void LidarSimulation::run(){
pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 1);
client_ = nh_.serviceClient<nav_msgs::GetMap>("/static_map");
updateMap(client_);
tf_.waitForTransform(global_frame_, lidar_frame_, ros::Time(), ros::Duration(1.0));
obstacle_service_ = nh_.advertiseService("/obstacle_handle", &LidarSimulation::obstacleHandleServer, this);
pub_laser_timer_ = nh_.createTimer(ros::Duration(1.0/rate_), &LidarSimulation::pubLaserCallback, this);
ros::spin();
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "lidar_simulation");
autolabor_simulation::LidarSimulation lidar;
lidar.run();
return 0;
}
3.1.3.2雷达模拟障碍物的数据
obstacle_simulation.cpp代码：
#include "tf/tf.h"
#include "lidar_simulation/obstacle_simulation.h"
#include "lidar_simulation/Obstacle.h"
namespace autolabor_simulation {
ObstacleSimulation::ObstacleSimulation(){
ros::NodeHandle private_node("~");
if (!private_node.getParam("obstacle_id", obstacle_id_)){
throw runtime_error("obstacle_id is not exist");
}
server_.reset(new interactive_markers::InteractiveMarkerServer(obstacle_id_));
private_node.param("inter_marker_id", inter_marker_id_, string("base_link"));
private_node.param("resolution", resolution_, 0.05);
XmlRpc::XmlRpcValue xml_obstacle_footprints;
if (private_node.getParam("obstacle_footprints", xml_obstacle_footprints)){
ROS_ASSERT(xml_obstacle_footprints.getType() == XmlRpc::XmlRpcValue::TypeArray);
for (int i=0; i<xml_obstacle_footprints.size(); i++){
ROS_ASSERT(xml_obstacle_footprints[i].getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(xml_obstacle_footprints[i].size() % 2 == 0);
geometry_msgs::Polygon polygon;
for (int j=0; j<xml_obstacle_footprints[i].size(); j=j+2){
ROS_ASSERT(xml_obstacle_footprints[i][j].getType() == XmlRpc::XmlRpcValue::TypeDouble);
ROS_ASSERT(xml_obstacle_footprints[i][j+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
geometry_msgs::Point32 p;
p.x = (float)(static_cast<double>(xml_obstacle_footprints[i][j]));
p.y = (float)(static_cast<double>(xml_obstacle_footprints[i][j+1]));
polygon.points.push_back(p);
}
obstacle_footprints_.push_back(polygon);
}
}
vector<geometry_msgs::Point> innerPoints;
for (int i=0; i<obstacle_footprints_.size(); i++){
getInnerPoint(obstacle_footprints_.at(i), innerPoints, resolution_);
}
visualization_msgs::InteractiveMarker interactive_marker_;
interactive_marker_.header.frame_id = inter_marker_id_;
interactive_marker_.header.stamp = ros::Time::now();
interactive_marker_.name = obstacle_id_;
visualization_msgs::Marker marker_;
marker_.type = visualization_msgs::Marker::CUBE_LIST;
marker_.scale.x = resolution_;
marker_.scale.y = resolution_;
marker_.scale.z = resolution_;
marker_.color.b = 1.0;
marker_.color.a = 1.0;
marker_.points = innerPoints;
visualization_msgs::InteractiveMarkerControl marker_control_;
marker_control_.always_visible = true;
marker_control_.markers.push_back(marker_);
marker_control_.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE;
marker_control_.orientation.w = 1.0;
marker_control_.orientation.y = 1.0;
interactive_marker_.controls.push_back(marker_control_);
visualization_msgs::InteractiveMarkerControl button_control_;
button_control_.always_visible = true;
button_control_.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
interactive_marker_.controls.push_back(button_control_);
server_->insert(interactive_marker_, boost::bind(&ObstacleSimulation::processFeedback, this, _1));
}
void ObstacleSimulation::getFootprintsRange(geometry_msgs::Polygon& footprint, float& min_x, float& max_x, float& min_y, float& max_y){
if (footprint.points.size() > 0){
min_x = max_x = footprint.points.at(0).x;
min_y = max_y = footprint.points.at(0).y;
for (int i=1; i<footprint.points.size(); i++){
min_x = footprint.points.at(i).x < min_x ? footprint.points.at(i).x : min_x;
max_x = footprint.points.at(i).x > max_x ? footprint.points.at(i).x : max_x;
min_y = footprint.points.at(i).y < min_y ? footprint.points.at(i).y : min_y;
max_y = footprint.points.at(i).y > max_y ? footprint.points.at(i).y : max_y;
}
}
}
void ObstacleSimulation::getInnerPoint(geometry_msgs::Polygon& footprint, std::vector<geometry_msgs::Point>& point_list, double resolution){
float min_x, max_x, min_y, max_y;
getFootprintsRange(footprint, min_x, max_x, min_y, max_y);
for (float x = (std::ceil(min_x / resolution) * resolution); x <= max_x; x = x + resolution){
for (float y = (std::ceil(min_y / resolution) * resolution); y <= max_y; y = y + resolution){
if (pnpoly(footprint, x, y)){
geometry_msgs::Point p;
p.x = x;
p.y = y;
point_list.push_back(p);
}
}
}
}
bool ObstacleSimulation::pnpoly(geometry_msgs::Polygon& footprint, float& x, float& y){
int i,j;
bool c = false;
for (i=0, j=footprint.points.size()-1; i<footprint.points.size(); j = i++){
if ( ( (footprint.points.at(i).y > y) != (footprint.points.at(j).y > y) ) &&
(x < (footprint.points.at(j).x-footprint.points.at(i).x) * (y-footprint.points.at(i).y) / (footprint.points.at(j).y - footprint.points.at(i).y) + footprint.points.at(i).x) ){
c = !c;
}
}
return c;
}
void ObstacleSimulation::processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
if (transform_flag_){
tf::Quaternion q;
q.setX(feedback->pose.orientation.x);
q.setY(feedback->pose.orientation.y);
q.setZ(feedback->pose.orientation.z);
q.setW(feedback->pose.orientation.w);
lidar_simulation::Obstacle msg;
msg.request.obstacle_id = obstacle_id_;
msg.request.type = lidar_simulation::Obstacle::Request::TRANSFORM;
msg.request.transform[0] = feedback->pose.position.x;
msg.request.transform[1] = feedback->pose.position.y;
msg.request.transform[2] = tf::getYaw(q);
client_.call(msg);
}
}
void ObstacleSimulation::menuFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
interactive_markers::MenuHandler::EntryHandle handle = feedback->menu_entry_id;
interactive_markers::MenuHandler::CheckState state;
menu_handler_.getCheckState(handle, state);
lidar_simulation::Obstacle msg;
msg.request.obstacle_id = obstacle_id_;
if (state == interactive_markers::MenuHandler::CHECKED){
msg.request.type = lidar_simulation::Obstacle::Request::DELETE;
if (client_.call(msg)){
menu_handler_.setCheckState(handle, interactive_markers::MenuHandler::UNCHECKED);
transform_flag_ = false;
}
}else{
tf::Quaternion q;
q.setX(feedback->pose.orientation.x);
q.setY(feedback->pose.orientation.y);
q.setZ(feedback->pose.orientation.z);
q.setW(feedback->pose.orientation.w);
msg.request.type = lidar_simulation::Obstacle::Request::NEW;
msg.request.vertex = obstacle_footprints_;
msg.request.transform[0] = feedback->pose.position.x;
msg.request.transform[1] = feedback->pose.position.y;
msg.request.transform[2] = tf::getYaw(q);
if (client_.call(msg)){
menu_handler_.setCheckState(handle, interactive_markers::MenuHandler::CHECKED);
transform_flag_ = true;
}
}
menu_handler_.reApply(*server_);
server_->applyChanges();
}
void ObstacleSimulation::initMenu(){
interactive_markers::MenuHandler::EntryHandle handle = menu_handler_.insert("Apply", boost::bind(&ObstacleSimulation::menuFeedback, this, _1));
menu_handler_.setCheckState(handle, interactive_markers::MenuHandler::UNCHECKED);
transform_flag_ = false;
}
void ObstacleSimulation::run(){
initMenu();
menu_handler_.apply(*server_, obstacle_id_);
client_ = nh_.serviceClient<lidar_simulation::Obstacle>("/obstacle_handle");
server_->applyChanges();
ros::spin();
}
}
int main(int argc, char **argv){
ros::init(argc, argv, "obstacle_simulation");
autolabor_simulation::ObstacleSimulation obstacle;
obstacle.run();
return 0;
}
3.1.3.3雷达模拟静态地图的数据
static_map.cpp代码：
#include "lidar_simulation/static_map.h"
namespace autolabor_simulation {
StaticMap::~StaticMap(){
if (data_length_ > 0){
delete [] map_data_;
}
}
void StaticMap::initMap(nav_msgs::OccupancyGrid& grid_map){
resolution_ = grid_map.info.resolution;
origin_x_ = grid_map.info.origin.position.x;
origin_y_ = grid_map.info.origin.position.y;
size_x_ = grid_map.info.width;
size_y_ = grid_map.info.height;
data_length_ = size_x_ * size_y_;
map_data_ = new char[data_length_];
obstacle_data_ = new char[data_length_];
for (int i=0; i<data_length_; i++){
map_data_[i] = grid_map.data.at(i);
}
exist_obstacle = false;
}
void StaticMap::mapToWorld(int mx, int my, float &wx, float &wy){
wx = origin_x_ + (mx + 0.5) * resolution_;
wy = origin_y_ + (my + 0.5) * resolution_;
}
void StaticMap::wordToMap(float wx, float wy, int &mx, int &my){
mx = (int)((wx - origin_x_) / resolution_);
my = (int)((wy - origin_y_) / resolution_);
}
char StaticMap::getValue(int mx, int my){
int index = my * size_x_ + mx;
if (index < 0 && index > data_length_){
return ERROR;
}else{
if (exist_obstacle){
return obstacle_data_[index] == OBS_SPACE ? OBS_SPACE : map_data_[index];
}else{
return map_data_[index];
}
}
}
char StaticMap::getValue(float wx, float wy){
int mx, my;
wordToMap(wx, wy, mx, my);
return getValue(mx, my);
}
void StaticMap::distance(int start_x, int start_y, double theta, int& end_x, int& end_y){
theta = normalAngle(theta);
double k, e=-0.5;
int major_step, minor_step;
if (limit_range(start_x, start_y)){
end_x = start_x;
end_y = start_y;
return;
}else if (std::abs(theta) <= (0.25 * M_PI) || std::abs(theta) >= (0.75 * M_PI)){
k = std::abs(std::tan(theta));
major_step = std::abs(theta) <= (0.25 * M_PI) ? 1 : -1;
minor_step = theta >= 0 ? 1 : -1;
while (true){
start_x += major_step;
e += k;
if (e >= 0){
start_y += minor_step;
e -= 1;
}
if (limit_range(start_x, start_y) || getValue(start_x, start_y) == OBS_SPACE || getValue(start_x, start_y) == UNKNOWN_SPACE){
end_x = start_x;
end_y = start_y;
return;
}
}
}else{
k = std::abs(tan(0.5 * M_PI - theta));
major_step = theta > 0 ? 1 : -1;
minor_step = std::abs(theta) <= (0.5 * M_PI) ? 1 : -1;
while (true){
start_y += major_step;
e += k;
if (e >= 0){
start_x += minor_step;
e -= 1;
}
if (limit_range(start_x, start_y) || getValue(start_x, start_y) == OBS_SPACE || getValue(start_x, start_y) == UNKNOWN_SPACE){
end_x = start_x;
end_y = start_y;
return;
}
}
}
}
void StaticMap::distance(float start_x, float start_y, double theta, float &end_x, float &end_y){
int start_mx, start_my, end_mx, end_my;
wordToMap(start_x, start_y, start_mx, start_my);
distance(start_mx, start_my, theta, end_mx, end_my);
mapToWorld(end_mx, end_my, end_x, end_y);
}
void StaticMap::getObstacleRange(footprint& footprint, float& min_x, float& max_x, float& min_y, float& max_y){
if (footprint.size() > 0){
min_x = max_x = footprint.at(0).x;
min_y = max_y = footprint.at(0).y;
for (int i=1; i<footprint.size(); i++){
min_x = footprint.at(i).x < min_x ? footprint.at(i).x : min_x;
max_x = footprint.at(i).x > max_x ? footprint.at(i).x : max_x;
min_y = footprint.at(i).y < min_y ? footprint.at(i).y : min_y;
max_y = footprint.at(i).y > max_y ? footprint.at(i).y : max_y;
}
min_x = std::max(min_x, (float)origin_x_);
max_x = std::min(max_x, (float)(origin_x_ + size_x_ * resolution_));
min_y = std::max(min_y, (float)origin_y_);
max_y = std::min(max_y, (float)(origin_y_ + size_y_ * resolution_));
}
}
bool StaticMap::pnpoly(footprint& footprint, float& x, float& y){
int i,j;
bool c = false;
for (i=0, j=footprint.size()-1; i<footprint.size(); j = i++){
if ( ( (footprint.at(i).y > y) != (footprint.at(j).y > y) ) &&
(x < (footprint.at(j).x-footprint.at(i).x) * (y-footprint.at(i).y) / (footprint.at(j).y - footprint.at(i).y) + footprint.at(i).x) ){
c = !c;
}
}
return c;
}
void StaticMap::getObstaclePoints(footprint& f, footprint& obs_points){
float min_x, max_x, min_y, max_y;
getObstacleRange(f, min_x, max_x, min_y, max_y);
for (float x = (std::ceil(min_x / resolution_) * resolution_); x <= max_x; x = x + resolution_/2){
for (float y = (std::ceil(min_y / resolution_) * resolution_); y <= max_y; y = y + resolution_/2){
if (pnpoly(f, x, y)){
Point p; p.x = x; p.y = y;
obs_points.push_back(p);
}
}
}
}
void StaticMap::deleteObstacle(string obstacle_name){
if (footprints_map_.find(obstacle_name) != footprints_map_.end()){
footprints_map_.erase(obstacle_name);
}
if (obstacle_points_map_.find(obstacle_name) != obstacle_points_map_.end()){
obstacle_points_map_.erase(obstacle_name);
}
if(obstacle_points_map_.size() == 0){
exist_obstacle = false;
}
}
void StaticMap::transformFootprint(footprint_list &source, footprint &target, float x, float y, float yaw){
float cos_th = cos(yaw);
float sin_th = sin(yaw);
footprint f;
for (int i=0; i<source.size(); i++){
f.clear();
for (int j=0; j<source[i].size(); j++){
Point p;
p.x = x + (source[i][j].x * cos_th - source[i][j].y * sin_th);
p.y = y + (source[i][j].x * sin_th + source[i][j].y * cos_th);
f.push_back(p);
}
getObstaclePoints(f, target);
}
}
void StaticMap::transformObstacle(string obstacle_name, float x, float y, float yaw){
footprints_map::iterator iter = footprints_map_.find(obstacle_name);
if (iter == footprints_map_.end()){
throw runtime_error("No specified obstacle");
}
if (obstacle_points_map_.find(obstacle_name) != obstacle_points_map_.end()){
obstacle_points_map_.erase(obstacle_name);
}
footprint obs_points;
transformFootprint(iter->second, obs_points, x, y, yaw);
obstacle_points_map_[obstacle_name] = obs_points;
}
void StaticMap::insertObstacle(string obstacle_name, vector<geometry_msgs::Polygon> polygon_list, float x, float y, float yaw){
deleteObstacle(obstacle_name);
footprint_list obstacle_footprints;
for (int i=0; i<polygon_list.size(); i++){
footprint f;
for (int j=0; j<polygon_list.at(i).points.size(); j++){
Point p;
p.x = polygon_list.at(i).points.at(j).x;
p.y = polygon_list.at(i).points.at(j).y;
f.push_back(p);
}
obstacle_footprints.push_back(f);
}
footprints_map_[obstacle_name] = obstacle_footprints;
footprint obstacle_points;
transformFootprint(obstacle_footprints, obstacle_points, x, y, yaw);
obstacle_points_map_[obstacle_name] = obstacle_points;
exist_obstacle = true;
}
void StaticMap::updateObstacleData(){
int mx, my, index;
memset(obstacle_data_, 0, data_length_);
for (obstacle_points_map::iterator iter = obstacle_points_map_.begin(); iter!=obstacle_points_map_.end(); iter++){
for (int i=0; i<(iter->second).size(); i++){
wordToMap((iter->second)[i].x, (iter->second)[i].y, mx, my);
index = my * size_x_ + mx;
if (index >=0 && index <= data_length_){
obstacle_data_[index] = OBS_SPACE;
}
}
}
}
double StaticMap::normalAngle(double theta){
if (theta > M_PI){
theta = normalAngle(theta - 2*M_PI);
}else if (theta <= -M_PI){
theta = normalAngle(theta + 2*M_PI);
}
return theta;
}
}
3.2、simulation_launch目录
3.2.1 小车地图创建
create_map_simulation.launch代码：
<launch>
<arg name="model" />
<arg name="gui" default="false" />
<param name="use_sim_time" value="false"/>
<param name="robot_description" textfile="$(find autolabor_description)/urdf/autolabor_description.urdf" />
<param name="use_gui" value="$(arg gui)" />
<node pkg="autolabor_fake" type="autolabor_fake_node" name="autolabor_driver" >
<remap from="/autolabor/cmd_vel" to="cmd_vel" />
<remap from="/autolabor/odom" to="odom" />
</node>
<node pkg="map_server" type="map_server" name="map_server" args="$(find simulation_launch)/map/custom_1.yaml" />
<node name="joy" pkg="joy" type="joy_node" />
<node name="joy_to_twist" pkg="joy_to_twist" type="joy_to_twist">
<param name="linear_min" value="0.3" />
<param name="linear_max" value="2.0" />
<param name="linear_step" value="0.1" />
<param name="angular_min" value="0.5" />
<param name="angular_max" value="3.0" />
<param name="angular_min" value="0.1" />
</node>
<node pkg="lidar_simulation" type="lidar_simulation" name="lidar" output="screen">
<param name="min_angle" value="-3.141592653"/>
<param name="max_angle" value="3.141592653" />
<param name="min_distance" value="0.15" />
<param name="max_distance" value="6.50" />
<param name="size" value="400"/>
<param name="global_frame" value="odom"/>
<param name="lidar_frame" value="lidar"/>
</node>
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="map_update_interval" value="1.0"/>
<param name="maxUrange" value="6.0"/>
<param name="maxRange" value="6.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.01"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="1.0"/>
<param name="lskip" value="0"/>
<param name="srr" value="0.5"/>
<param name="srt" value="0.2"/>
<param name="str" value="0.3"/>
<param name="stt" value="0.2"/>
<param name="linearUpdate" value="0.01"/>
<param name="angularUpdate" value="0.01"/>
<param name="particles" value="15"/>
<param name="xmin" value="-20.0"/>
<param name="ymin" value="-20.0"/>
<param name="xmax" value="20.0"/>
<param name="ymax" value="20.0"/>
<param name="delta" value="0.05"/>
<param name="minimumScore" value="100"/>
<param name="temporalUpdate" value="3.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find autolabor_description)/urdf.rviz" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.20 0.0 0.0 0.0 /base_link /lidar 10" />
</launch>
3.2.2小车自动规划
move_base_simulation.launch代码：
<launch>
<arg name="model" />
<arg name="gui" default="false" />
<param name="use_sim_time" value="false"/>
<param name="robot_description" textfile="$(find autolabor_description)/urdf/autolabor_description.urdf" />
<param name="use_gui" value="$(arg gui)" />
<node pkg="autolabor_fake" type="autolabor_fake_node" name="autolabor_driver" >
<remap from="/autolabor/cmd_vel" to="cmd_vel" />
<remap from="/autolabor/odom" to="odom" />
</node>
<node pkg="map_server" type="map_server" name="map_server" args="$(find simulation_launch)/map/MG_map.yaml" />
<node pkg="lidar_simulation" type="lidar_simulation" name="lidar" output="screen">
<param name="min_angle" value="-3.141592653"/>
<param name="max_angle" value="3.141592653" />
<param name="min_distance" value="0.15" />
<param name="max_distance" value="6.50" />
<param name="size" value="400"/>
<param name="global_frame" value="odom"/>
<param name="lidar_frame" value="lidar"/>
</node>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<param name="use_map_topic" value="false"/>
<param name="odom_model_type" value="diff"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_min_range" value="0.2"/>
<param name="laser_max_range" value="6.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.02"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.95"/>
<param name="laser_z_short" value="0.025"/>
<param name="laser_z_max" value="0.025"/>
<param name="laser_z_rand" value="0.05"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.25"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="odom"/>
<param name="base_frame_id" value="base_link"/>
<param name="global_frame_id" value="map"/>
<param name="resample_interval" value="3"/>
<param name="transform_tolerance" value="0.5"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_cov_xx" value="0.25"/>
<param name="initial_cov_yy" value="0.25"/>
<param name="initial_cov_aa" value="10.0"/>
</node>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find simulation_launch)/param/move_base_params.yaml" command="load" />
<rosparam file="$(find simulation_launch)/param/global_costmap_params.yaml" command="load" ns="global_costmap"/>
<rosparam file="$(find simulation_launch)/param/local_costmap_params.yaml" command="load" ns="local_costmap"/>
<rosparam file="$(find simulation_launch)/param/global_planner_params.yaml" command="load" ns="GlobalPlanner"/>
<rosparam file="$(find simulation_launch)/param/dwa_local_planner_params.yaml" command="load" ns="DWAPlannerROS"/>
</node>
<node name="obstacle_simulation" pkg="lidar_simulation" type="obstacle_simulation" output="screen">
<param name="obstacle_id" value="obstacle" />
<param name="inter_marker_id" value="map" />
<param name="resolution" value="0.05" />
<rosparam param="obstacle_footprints">[[0.1,1.2, 0.1,-1.2, -0.1,-1.2, -0.1,1.2]]</rosparam>
</node>
<node name="obstacle_simulation_0" pkg="lidar_simulation" type="obstacle_simulation" output="screen">
<param name="obstacle_id" value="obstacle_0" />
<param name="inter_marker_id" value="map" />
<param name="resolution" value="0.05" />
<rosparam param="obstacle_footprints">[[0.0,0.0, 0.,0.5, 0.5,0.5, 0.5,0.0]]</rosparam>
</node>
<node name="obstacle_simulation_2" pkg="lidar_simulation" type="obstacle_simulation" output="screen">
<param name="obstacle_id" value="obstacle_2" />
<param name="inter_marker_id" value="map" />
<param name="resolution" value="0.05" />
<rosparam param="obstacle_footprints">[[0.0,0.0, 0.0,0.5, 0.5,0.0, 0.2,0.0, 0.2,-0.4, 0.0,-0.4]]</rosparam>
</node>
<node name="obstacle_simulation_1" pkg="lidar_simulation" type="obstacle_simulation" output="screen">
<param name="obstacle_id" value="obstacle_1" />
<param name="inter_marker_id" value="map" />
<param name="resolution" value="0.05" />
<rosparam param="obstacle_footprints">[[0.1,1.2, 0.1,-1.2, -0.1,-1.2, -0.1,1.2]]</rosparam>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find autolabor_description)/urdf.rviz" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.20 0.0 0.0 0.0 /base_link /lidar 10" />
</launch>
3.2.3用键盘或手柄控制模拟小车运动
sim_move_simulation.launch代码：
<launch>
<arg name="model" />
<arg name="gui" default="false" />
<param name="use_sim_time" value="false"/>
<param name="robot_description" textfile="$(find autolabor_description)/urdf/autolabor_description.urdf" />
<param name="use_gui" value="$(arg gui)" />
<node pkg="autolabor_fake" type="autolabor_fake_node" name="autolabor_driver" >
<remap from="/autolabor/cmd_vel" to="cmd_vel" />
<remap from="/autolabor/odom" to="odom" />
</node>
<node pkg="map_server" type="map_server" name="map_server" args="$(find simulation_launch)/map/custom.yaml" />
<node name="joy" pkg="joy" type="joy_node" />
<node name="joy_to_twist" pkg="joy_to_twist" type="joy_to_twist">
<param name="linear_min" value="0.3" />
<param name="linear_max" value="2.0" />
<param name="linear_step" value="0.1" />
<param name="angular_min" value="0.5" />
<param name="angular_max" value="3.0" />
<param name="angular_min" value="0.1" />
</node>
<node name="keyboard_control" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen"/>
<node pkg="lidar_simulation" type="lidar_simulation" name="lidar" output="screen">
<param name="min_angle" value="-1.570796327"/>
<param name="max_angle" value="1.570796327" />
<param name="min_distance" value="0.15" />
<param name="max_distance" value="10.00" />
<param name="size" value="400"/>
<param name="rate" value="10"/>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find autolabor_description)/urdf.rviz" />
<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0.0 0.0 0.0 /map /odom 10" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.20 0.0 0.0 0.0 /base_link /lidar 10" />
</launch>
3.3、小车自主规划的算法及navigation组件
move_base_sim 小车自主规划的算法，以及navigation组件，这个主要是基于ROS官网上navigation包加以修改适应模拟器来的。
四、仿真运行
4.1、加载出Rviz模拟画面
进入工作空间catkin_ws的文件夹下，右键选择在终端打开，再次使用catkin_make命令对工作空间进行编译。
或者打开终端使用命令：
cd ~/catkin_ws/
catkin_make
注意：源码创建完成后第一次编译工作站需要时间较长
（如果ros第一次运行该项目，则会提示如下错误信息，以下提示为缺少导航包，则只需要根据报错信息中表示缺相关依赖的组件，来安装相应的包即可：$ sudo apt-get install ros-kinetic-navigation 然后再次进行编译即可）
刷新一次环境：
source ~/catkin_ws/devel/setup.bash
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
运行launch文件，启动模拟器，加载出Rviz模拟画面：
roslaunch simulation_launch move_base_simulation.launch
4.2、仿真
4.2.1自动规划一条路径
在模拟器可视化窗口程序中，选择2D Nav Goal选项，将鼠标移到动迷宫地图中白色区域的任意一点，然后点击确定目标位置，释放鼠标，就能设定机器人小车将要行驶的目标点，机器人小车就会自动规划一条路径，并按照规划的路线行驶。
4.2.2躲避障碍物
4.2.2.1添加障碍物
点击左下角的Add选项，弹出添加topic的对话框，然后选择By topic下面的/obstacle下的interactiveMarkers，点击OK，即可将障碍物添加到其中。
选中障碍物右键将Apply的复选框的勾选上：
4.2.2.2设置、躲避障碍物
用鼠标左键选中动态障碍物，拖拽并移动到你需要放置的地方，当机器人小车运动到障碍物附近时，机器人小车上的激光雷达会采集它的信息并进行判断，进而重新规划路径。
放置障碍物，机器人未遇到障碍物前规划的行走路径和方向：
机器人即将遇到障碍物：
机器人遇到障碍物后重新规划的行走路径和方向：
296
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
