宇树机器狗go2—slam建图(2)gmapping

0.前言

上一篇番外文章带大家认识了一下slam中常用的一些点云格式，本期文章会教大家如何让宇树的机器狗go2在仿真环境中使用gnaaping进行2d建图，在后续的slam建图和slam算法解析的时候会经常与这些点云信息打交道。宇树机器狗go2仿真会作为一个长期项目来进行不定期更新，仿真不只限于在gazebo上实现，后面还会在isaac sim等仿真平台上教大家如何搭建，其目的还是帮助各位都能以较低的成本加入到宇树机器狗go2的学习中。gazebo的仿真对电脑的需求会低于isaac sim等仿真平台，这对实验条件要求会低很多，更很适合学校小实验室或个人进行仿真实验。

同时我也希望能有更多的开发者加入近来一起完成机器狗go2的仿真实验项目的建设，如有更好的方案和建议可以私聊或申请共创作者。

1.机器狗go2的cmd_vel和odom

1.启动gazebo仿真

roslaunch unitree_guide gazeboSim.launch

2.启动控制器

这里推荐使用最高权限，按下按键2和按键5后会进入move_base模式

./devel/lib/unitree_guide/junior_ctrl
或着
rosrun unitree_guide junior_ctrl 
最高权限
sudo ./devel/lib/unitree_guide/junior_ctrl

3.使用rostopic的cmd_vel对go2进行控制

在move_base模式下会不断的接收cmd_vel消息，随后将cmd_vel的角速度和线速度赋值到go2的运动速度上进而控制go2的运动，具体代码在FSM(有限状态机)的state_move_base.cpp中：

代码如下：

void State_move_base::twistCallback(const geometry_msgs::Twist& msg){_vx = msg.linear.x;_vy = msg.linear.y;_wz = msg.angular.z;
}

move_base模式和trotting模式几乎一致，只是将go2的运动控制切换成rostopic下cmd_vel话题进行运动控制，因此如果需要使用ros进行导航和路径规划则需要将go2的状态切换至move_base状态，同时在move_base模式下我们也可以使用cmd_vel话题进行机器狗的简单运动控制了

4.go2的odom

在启动控制器后go2的里程计就已经被启动了,odom的发布代码在估计器的代码中。

代码在unitree_guide/unitree_guide/src/control下的Estimator.cpp里面。

    #ifdef COMPILE_WITH_MOVE_BASEif(_count % ((int)( 1.0/(_dt*_pubFreq))) == 0){_currentTime = ros::Time::now();/* tf */_odomTF.header.stamp = _currentTime;_odomTF.header.frame_id = "odom";_odomTF.child_frame_id  = "base";_odomTF.transform.translation.x = _xhat(0);_odomTF.transform.translation.y = _xhat(1);_odomTF.transform.translation.z = _xhat(2);_odomTF.transform.rotation.w = _lowState->imu.quaternion[0];_odomTF.transform.rotation.x = _lowState->imu.quaternion[1];_odomTF.transform.rotation.y = _lowState->imu.quaternion[2];_odomTF.transform.rotation.z = _lowState->imu.quaternion[3];_odomBroadcaster.sendTransform(_odomTF);/* odometry */_odomMsg.header.stamp = _currentTime;_odomMsg.header.frame_id = "odom";_odomMsg.pose.pose.position.x = _xhat(0);_odomMsg.pose.pose.position.y = _xhat(1);_odomMsg.pose.pose.position.z = _xhat(2);_odomMsg.pose.pose.orientation.w = _lowState->imu.quaternion[0];_odomMsg.pose.pose.orientation.x = _lowState->imu.quaternion[1];_odomMsg.pose.pose.orientation.y = _lowState->imu.quaternion[2];_odomMsg.pose.pose.orientation.z = _lowState->imu.quaternion[3];_odomMsg.pose.covariance = _odom_pose_covariance;_odomMsg.child_frame_id = "base";_velBody = _rotMatB2G.transpose() * _xhat.segment(3, 3);_wBody   = _lowState->imu.getGyro();_odomMsg.twist.twist.linear.x = _velBody(0);_odomMsg.twist.twist.linear.y = _velBody(1);_odomMsg.twist.twist.linear.z = _velBody(2);_odomMsg.twist.twist.angular.x = _wBody(0);_odomMsg.twist.twist.angular.y = _wBody(1);_odomMsg.twist.twist.angular.z = _wBody(2);_odomMsg.twist.covariance = _odom_twist_covariance;_pub.publish(_odomMsg);_count = 1;}/* ROS odometry publisher */#ifdef COMPILE_WITH_MOVE_BASE_pub = _nh.advertise<nav_msgs::Odometry>("odom", 1);#endif  // COMPILE_WITH_MOVE_BASE

2.gmapping建图

1.获取gmaaping功能包

在了解了机器狗go2的里程计和cmd_vel是怎样发布后，就可以开始使用gmapping进行slam建图了，先安装gmapping功能包

sudo apt-get install ros-noetic-gmapping

2.编写launch文件

在文件夹unitree_guide/unitree_guide/launch下创建go2_gmapping.launch文件

touch go2_gmapping.launch

 打开go2_gmapping.launch文件

gedit go2_gmapping.launch

将下方描述添加到文件中

<launch><node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"><param name="base_frame" value="base"/> <!--机器人底盘坐标系基框架，附带在移动底盘的框架，原点--><param name="odom_frame" value="odom"/> <!--里程计坐标系里程计框架，附带在里程计的框架--><param name="map_frame" value="map"/> <!--地图坐标系地图框架，附带在地图上的框架--><param name="map_update_interval" value="0.01"/><!--地图更新速度，秒0.01--><param name="maxUrange" value="10.0"/><!--激光最大可用距离--><param name="maxRange" value="12.0"/><!--zuida juli-->        <param name="sigma" value="0.05"/><param name="kernelSize" value="3"/><!--moren:1--><param name="lstep" value="0.05"/><param name="astep" value="0.05"/><param name="iterations" value="5"/><param name="lsigma" value="0.075"/><param name="ogain" value="3.0"/><param name="lskip" value="0"/><param name="srr" value="0.1"/><param name="srt" value="0.2"/><param name="str" value="0.1"/><param name="stt" value="0.2"/><param name="minimumScore" value="0"/><param name="linearUpdate" value="0.05"/><!--线速度角速度在地图的更新--><param name="angularUpdate" value="0.0436"/><param name="temporalUpdate" value="-1"/><!--moren:-1--><param name="resampleThreshold" value="0.5"/><param name="particles" value="8"/><!--moren:30 gaicheng:8--><param name="xmin" value="-50.0"/><param name="ymin" value="-50.0"/><param name="xmax" value="50.0"/><param name="ymax" value="50.0"/><param name="delta" value="0.05"/><param name="llsamplerange" value="0.01"/> <param name="llsamplestep" value="0.01"/><param name="lasamplerange" value="0.005"/><param name="lasamplestep" value="0.005"/><!--param name="transform_publish_period" value="0.01"/--></node>
</launch>

3.建图测试

1.启动仿真

这里我们并没有更换地图场景（仅为测试使用），后面我们会更换地图场景

roslaunch unitree_guide gazeboSim.launch 

2.启动控制器

这里推荐使用最高权限

./devel/lib/unitree_guide/junior_ctrl
或着
rosrun unitree_guide junior_ctrl 
最高权限
sudo ./devel/lib/unitree_guide/junior_ctrl

按下按键2后按下按键4，这会使得go2的有限状态机将控制状态更换为stand_trotting

3.启动gmapping进行slam建图

启动我们在上方创建的gmapping建图的launch文件。

roslaunch unitree_guide go2_gmapping.launch 

4.打开rviz并进行配置

rviz

点击下方添加add

添加地图

点击宇树机器狗go2—slam建图(2)gmapping查看全文