【自动驾驶】基于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 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
#include "sensor_msgs/LaserScan.h"
#include "husky_highlevel_controller/husky_controller.hpp"
#include
#include
#include
#include
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
#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代码：
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