ROS控制 从移动指定距离流畅切换到自主导航

介绍：
MoveTargetDistanceForward是个移动指定距离的函数。
PublishGoalCmd时move_base中发送导航坐标，自主导航到目标点的函数。

需求：向前移动0.3m后，直接切换到导航状态，但机器人会卡顿一下。
因此需要进行多线程同时启动两个函数，同时还需要设置信号量，控制两个函数的优先级。
让MoveTargetDistanceForward先执行，PublishGoalCmd也同时规划好路径。
让导航的速度会被暂时忽视，等待MoveTargetDistanceForward执行完成，再接收导航速度。

MoveTargetDistanceForward

-> cmd_vel_for_moto -> cmd_vel
将速度发给cmd_vel_for_moto，然后由CmdVelRelayNode控制，转发给cmd_vel

• 修改deskmedia/src/deskmedia_node.cpp

// 向cmd_vel_for_moto发送速度
cmd_vel_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel_for_moto", 1);

PublishGoalCmd

-> cmd_vel_for_goal -> cmd_vel
将速度发给cmd_vel_for_goal，然后由CmdVelRelayNode控制，等待MoveTargetDistanceForward完成，再转发给cmd_vel

1. 添加src/navigation-melodic/move_base/include/move_base/move_base.h

      std::string cmd_vel_;

2. 修改navigation-melodic/move_base/src/move_base.cpp

	// 接受turn_on_wheeltec_robot/param_common/move_base_params.yaml的cmd_vel，默认参数为cmd_vel
	// 但在yaml中设置为cmd_vel_for_goal
    private_nh.param("cmd_vel", cmd_vel_, std::string("cmd_vel"));
    vel_pub_ = nh.advertise<geometry_msgs::Twist>(cmd_vel_, 1);

3. 添加turn_on_wheeltec_robot/param_common/move_base_params.yaml

cmd_vel: "cmd_vel_for_goal" 

新建deskmedia/src/CmdVelRelayNode.cpp 转发速度

1. 订阅cmd_vel_for_moto和cmd_vel_for_goal
2. 接收到cmd_vel_for_moto发来的信息，将isCmdVelProcessing=true，表示占用，转发速度给cmd_vel，设置定时器0.1s,如果没接收到cmd_vel_for_moto的消息，就将isCmdVelProcessing=false，表示解除占用。
3. 接收cmd_vel_for_goal的信息，检查isCmdVelProcessing是否在占用，在占用，则直接返回;不在占用，转发信息给cmd_vel

核心：
多线程启动MoveTargetDistanceForward和PublishGoalCmd。