您的位置：首页 > 其它

用自己的机器人创建地图---40

2016-05-25 17:25 176 查看

原创博客:转载请标明出处:http://www.cnblogs.com/zxouxuewei/

时间过去还久了之前有其他项目的事情耽误了博客的更新，今天有点时间了，打算更新一贴，也是大家比较喜欢的一个贴。就是用自己搭建的机器人移动平台去创建一个地图。

首先要求如下：

　　1.必须要有一个能够遥控或者自动的移动平台，并且已经经过速度闭环控制。（我的基控制器代码没有开源出来，后期会讲到相关的PID控制和基本的boost串口通行问题）

　　2.校准了角速度和线速度。（http://www.cnblogs.com/zxouxuewei/p/5482302.html）

　　3.必须可以发布激光数据（可以采用KINECT或者激光雷达）。

　　4.必须要发布TF树的转换，odom---->base_link---->laser(基本就可以了，发布时有些直接可以静态发布)。

我选取的场地和机器人如下：

实际建图的效果如下：（一个房间和走廊的地图）

一。操作如下：

1.我的机器人的启动文件如下：

<launch>
<paramname="/use_sim_time"value="$(argsimulation)"/>
<nodename="base_laser_tf"pkg="tf"type="static_transform_publisher"args="0.1500.15000base_linklaser100"/>
<nodepkg="odom_tf_package"type="tf_broadcaster_node"name="tf_broad"/>
<includefile="$(findodom_tf_package)/launch/include/rplidar_ros.launch.xml">
</include>
</launch>

代码如下：tf_broadcaster_node.cpp

#include<ros/ros.h>
#include<sensor_msgs/JointState.h>
#include<tf/transform_broadcaster.h>
#include<nav_msgs/Odometry.h>
#include<iostream>
#include<iomanip>
#include<boost/asio.hpp>
#include<boost/bind.hpp>
#include<math.h>
#include"string.h"

usingnamespacestd;
usingnamespaceboost::asio;

doublex=0.0;
doubley=0.0;
doubleth=0.0;
doublevx=0.0;
doublevy=0.0;
doublevth=0.0;
doubledt=0.0;

//Definesthepacketprotocolforcommunicationbetweenupperandlowercomputers
#pragmapack(1)
typedefunion_Upload_speed_
{
unsignedcharbuffer[16];
struct_Speed_data_
{
floatHeader;
floatX_speed;
floatY_speed;
floatZ_speed;
}Upload_Speed;
}Struct_Union;
#pragmapack(4)

//Initializestheprotocolpacketdata
Struct_UnionReciver_data={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
Struct_UnionTransmission_data={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

//Definesthemessagetypetobetransmittedgeometry_msgs
geometry_msgs::Quaternionodom_quat;

voidcmd_velCallback(constgeometry_msgs::Twist&twist_aux)
{
geometry_msgs::Twisttwist=twist_aux;

Transmission_data.Upload_Speed.Header=header_data;
Transmission_data.Upload_Speed.X_speed=twist_aux.linear.x;
Transmission_data.Upload_Speed.Y_speed=twist_aux.linear.y;
Transmission_data.Upload_Speed.Z_speed=twist_aux.angular.z;
}

intmain(intargc,char**argv)
{
unsignedcharcheck_buf[1];
std::stringusart_port,robot_frame_id,smoother_cmd_vel;
intbaud_data;
floatp,i,d;

ros::init(argc,argv,"base_controller");
ros::NodeHandlen;
ros::Timecurrent_time,last_time;

//Getstheparametersinthelaunchfile
ros::NodeHandlenh_private("~");
nh_private.param<std::string>("usart_port",usart_port,"/dev/robot_base");
nh_private.param<int>("baud_data",baud_data,115200);
nh_private.param<std::string>("robot_frame_id",robot_frame_id,"base_link");
nh_private.param<std::string>("smoother_cmd_vel",smoother_cmd_vel,"/cmd_vel");

//Createabootnodefortheunderlyingdriverlayeroftherobotbase_controller
ros::Subscribercmd_vel_sub=n.subscribe(smoother_cmd_vel,50,cmd_velCallback);
ros::Publisherodom_pub=n.advertise<nav_msgs::Odometry>("odom",50);
tf::TransformBroadcasterodom_broadcaster;

//Initializesthedatarelatedtotheboostserialport
io_serviceiosev;
serial_portsp(iosev,usart_port);
sp.set_option(serial_port::baud_rate(baud_data));
sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
sp.set_option(serial_port::parity(serial_port::parity::none));
sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
sp.set_option(serial_port::character_size(8));

while(ros::ok())
{
ros::spinOnce();
//Getsthecycleoftwotimeslicerotations.Thepurposeistocalculatetheodommessagedata
current_time=ros::Time::now();
dt=(current_time-last_time).toSec();
last_time=ros::Time::now();

//Readthedatafromthelowercomputer
read(sp,buffer(Reciver_data.buffer));
if(Reciver_data.Upload_Speed.Header>15.4&&Reciver_data.Upload_Speed.Header<15.6)
{
vx=Reciver_data.Upload_Speed.X_speed;
vy=Reciver_data.Upload_Speed.Y_speed;
vth=Reciver_data.Upload_Speed.Z_speed;
//ROS_INFO("%2f%2f%2f",vx,vy,vth);
}
else
{
//ROS_INFO("------Pleasewaitwhiletherobotisconnected!-----");
read(sp,buffer(check_buf));
}
//Sendthenextbitmachineunderthecmd_valtopictomonitorthespeedinformation
write(sp,buffer(Transmission_data.buffer,16));
//Calculateodometerdata
doubledelta_x=(vx*cos(th)-vy*sin(th))*dt;
doubledelta_y=(vx*sin(th)+vy*cos(th))*dt;
doubledelta_th=vth*dt;
x+=delta_x;
y+=delta_y;
th+=delta_th;

geometry_msgs::Quaternionodom_quat=tf::createQuaternionMsgFromYaw(th);

geometry_msgs::TransformStampedodom_trans;
odom_trans.header.stamp=current_time;
odom_trans.header.frame_id="odom";
odom_trans.child_frame_id=robot_frame_id;

odom_trans.transform.translation.x=x;
odom_trans.transform.translation.y=y;
odom_trans.transform.translation.z=0.0;
odom_trans.transform.rotation=odom_quat;

odom_broadcaster.sendTransform(odom_trans);

nav_msgs::Odometryodom;
odom.header.stamp=current_time;
odom.header.frame_id="odom";

odom.pose.pose.position.x=x;
odom.pose.pose.position.y=y;
odom.pose.pose.position.z=0.0;
odom.pose.pose.orientation=odom_quat;

odom.child_frame_id=robot_frame_id;
odom.twist.twist.linear.x=vx;
odom.twist.twist.linear.y=vy;
odom.twist.twist.angular.z=vth;
//Releaseodometerdatabetweenodom->base_link
odom_pub.publish(odom);
}
iosev.run();
}

2.gmapping启动launch文件如下：

<launch>

<argname="scan_topic"default="scan"/>
<nodepkg="gmapping"type="slam_gmapping"name="slam_gmapping"output="screen"clear_params="true">
<paramname="odom_frame"value="odom"/>
<paramname="map_update_interval"value="2.0"/>
<!--SetmaxUrange<actualmaximumrangeoftheLaser-->
<paramname="maxRange"value="5.0"/>
<paramname="maxUrange"value="4.5"/>
<paramname="sigma"value="0.05"/>
<paramname="kernelSize"value="1"/>
<paramname="lstep"value="0.05"/>
<paramname="astep"value="0.05"/>
<paramname="iterations"value="5"/>
<paramname="lsigma"value="0.075"/>
<paramname="ogain"value="3.0"/>
<paramname="lskip"value="0"/>
<paramname="srr"value="0.01"/>
<paramname="srt"value="0.02"/>
<paramname="str"value="0.01"/>
<paramname="stt"value="0.02"/>
<paramname="linearUpdate"value="0.5"/>
<paramname="angularUpdate"value="0.5"/>
<paramname="temporalUpdate"value="-1.0"/>
<paramname="resampleThreshold"value="0.5"/>
<paramname="particles"value="80"/

<!--
<paramname="xmin"value="-50.0"/>
<paramname="ymin"value="-50.0"/>
<paramname="xmax"value="50.0"/>
<paramname="ymax"value="50.0"/>
makethestartingsizesmallforthebenefitoftheAndroidclient'smemory...
-->
<paramname="xmin"value="-1.0"/>
<paramname="ymin"value="-1.0"/>
<paramname="xmax"value="1.0"/>
<paramname="ymax"value="1.0"/>

<paramname="delta"value="0.05"/>
<paramname="llsamplerange"value="0.01"/>
<paramname="llsamplestep"value="0.01"/>
<paramname="lasamplerange"value="0.005"/>
<paramname="lasamplestep"value="0.005"/>
<remapfrom="scan"to="$(argscan_topic)"/>
</node>
</launch>

3.rviz启动文件配置如下：

<launch>
<nodename="rplidarNode"pkg="rplidar_ros"type="rplidarNode"output="screen">
<paramname="serial_port"type="string"value="/dev/ttyUSB0"/>
<paramname="serial_baudrate"type="int"value="115200"/>
<paramname="frame_id"type="string"value="laser"/>
<paramname="inverted"type="bool"value="false"/>
<paramname="angle_compensate"type="bool"value="true"/>
</node>
</launch>

二。开始创建地图。

1，首先启动机器人的节点：

roslaunchodom_tf_packageZHXWBOT_start.launch

2.启动机器人控制节点（如果自己有遥控器就没有必要启动）

arbotix_gui

3.启动gmapping

roslaunchodom_tf_packagegmapping.launch

4.启动rviz实时查看创建地图的过程

roslaunchodom_tf_packagerviz.launch

然后通过机器人控制节点移动你的机器人到达所建场地的每一个角落，尽量走一个闭合的路线，

内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理

标签：

相关文章推荐

新的分享

章节导航