【自动驾驶】基于Husky机器人的小案例

教程基于《无人驾驶原理与实践》2.6.4，请结合此书学习。

先指出书中有3点错误：

1.创建的包名为husky_highlevel_controller，但后文和程序中中多次变成husky_high_level_controller，统一用一个名称即可。
本文用的husky_highlevel_controller，涉及修改的地方有include/husky_high_level_controller目录，CmakeLists.txt内容，husky_highlevel_controller_node.app内容、husky_controller.cpp内容、launch文件内容。

2.代码清单2-13，CMakeList.txt:

catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}    #改正：这句话取消注释
   CATKIN_DEPENDS roscpp sensor_msgs
#  DEPENDS system_lib
)

3代码清单2-18

roslaunch husky_highlevel_controller high_controller.launch 		# 这句话取消注释，才能启动launch文件

ok，开始教程！
————————————————————————————————————————————————

1.创建ROS包：

cd ~/catkin_ws/src
catkin_create_pkg husky_highlevel_controller roscpp sensor_msgs

2.在husky_highlevel_controller/src/下创建2个源文件：

cd ~/catkin_ws/src/husky_highlevel_controller/src/
touch husky_highlevel_controller_node.cpp husky_controller.cpp

husky_controller.cpp代码：

#include "husky_highlevel_controller/husky_controller.hpp"

namespace husky_controller{
	
	HuskyController::HuskyController(ros::NodeHandle &node_handle):nodeHandle_(node_handle){
		std::string topic;
		if (! nodeHandle_.getParam("/laser_listener/laser_topic", topic)){
			ROS_ERROR("Load the laser scan topic param fail!!");
		}else{
			HuskyController::laserSub_ =nodeHandle_.subscribe(topic, 1, &HuskyController::LaserCallBack, this);
			
			ros::spin();
		}
	}
	
	void HuskyController::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr&msg){
		unsigned long len = msg -> ranges.size();
		std::vector<float> filtered_scan;
		for (int i=0; i < len; ++i){
			if (std::isnormal(msg->ranges[i])){
				filtered_scan.push_back(msg->ranges[i]);
			}
		}
		for (int j=0; j < filtered_scan.size(); ++j){
			ROS_INFO_STREAM(filtered_scan[j]);
		}
	}

} //namespace husky_controller

husky_highlevel_controller_node.cpp代码：

#include <ros/ros.h>
#include "sensor_msgs/LaserScan.h"
#include "husky_highlevel_controller/husky_controller.hpp"
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <iostream>

using namespace std;

int main(int argc, char ** argv){
	ros::init(argc, argv, "laser_listener");
	
	ros::NodeHandle node_handle;
	husky_controller::HuskyController test(node_handle);
	
	return 0;
}

3.创建头文件：

cd ~/catkin_ws/src/husky_highlevel_controller/include/husky_highlevel_controller/
touch husky_controller.hpp

husky_controller.hpp代码：

#include <ros/ros.h>
#include "sensor_msgs/LaserScan.h"

namespace husky_controller{
	
	class HuskyController{
		public:
			HuskyController(ros::NodeHandle &node_handle);
			
		private:
			void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr &msg);
			ros::NodeHandle &nodeHandle_;
			ros::Subscriber laserSub_;
	};

} //namespace husky_controller

4.修改CMakeList.txt，文件在~/catkin_ws/src/huskg_highlevel_controller/路径下

修改为如下内容：

cmake_minimum_required(VERSION 3.0.2)
project(husky_highlevel_controller)
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
)

catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
   CATKIN_DEPENDS roscpp sensor_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp src/husky_controller.cpp)

target_link_libraries(${PROJECT_NAME}_node
   ${catkin_LIBRARIES}
 )

5.在包路径下新建launch文件夹，在文件夹里创建high_controller.launch文件

high_controller.launch代码：

<launch>
	<include file="$(find husky_gazebo)/launch/husky_playpen.launch">
		<arg name="laser_enabled" value="true"/>
	</include>
	
	<node pkg="husky_highlevel_controller" type="husky_highlevel_controller_node" name="laser_listener" output="screen">
		<param name="laser_topic" value="/scan"/>
	</node>
	
	<node pkg="rviz" type="rviz" name="rviz"/>
</launch>

6.编译ROS包：

cd ~/catkin_ws
catkin_make

正常编译过程如上图，最后会在~/catkin/build/文件夹下生成编译结果：

7.启动launch文件

roslaunch husky_highlevel_controller high_controller.launch

正常启动会弹出RVIZ和Gazebo软件界面，

然后设置一下RVIZ：
（1）Fixed Frame修改为odom；
（2）点击左下角“Add”按钮，添加LaserSan选项，然后将LaserScan 的Topic设置为**/scan**，size设置为0.1

然后中间会显示出如图所示的一些线条（根据电脑性能，可能需要等一会才能显示）

另一边，Gazebo会显示出下图界面（最中心处的那个矩形就是我们的Husky无人车了，怎么控制它移动暂时还不会）

同时，回到启动launch的终端里，会发现不停的显示激光的扫描数据：

OK，此教程结束！
若需要打包后的全部源码，可以到https://download.csdn.net/download/xiangxiang613/13184023下载

补充：可能遇到的问题及解决办法
————————————————————————————————————————————

1.catkin_make报错：Permission denied
以下面的为例，不针对此教程：

解决办法：修改权限
sudo chmod ugo+wx build/.built_by

2.启动launch文件报错：Invalid roslaunch XML syntax: not well-formed (invalid token)

解决办法：这个是因为launch文件里的语法书写错误，根据对应的行和列定位和纠正问题即可。

附launch语法：https://www.cnblogs.com/wjyHIT2011/articles/6833388.html