最常见的移动机器人模型，差速，麦克纳姆轮的全向，阿克曼车式等。 
直道行驶–视觉 
弯道行驶–视觉
全国大学生智能汽车竞赛-室外光电组无人驾驶挑战赛-2019 https://blog.csdn.net/ZhangRelay/article/details/89639965
仿真环境（激光雷达lidar）： 
实车测试（激光雷达lidar）： 
左转和右转半径示意
主流的仿真软件都提供了这些模型的机器人，如webots,V-rep,Gazebo. 
结构示意如上图。 重点参考：
部分代码只支持遥控，并不支持此类（ackermann）模型的导航，原生的ROS导航包也不能直接使用，需要调整。 ROS官网wiki有ackermann兴趣小组：为Ackermann机器人的控制，导航和模拟仿真生成通用接口。 teb_local_planner stepback_and_steerturn_recovery
1.设置并测试优化 在本教程中，学习如何运行轨迹优化以及如何更改基础参数以设置自定义行为和性能。 2.检查优化反馈 在本教程中，学习如何检查优化轨迹的反馈; 提供了一个示例，其可视化当前所选轨迹的速度分布。 3.配置并运行机器人导航 在本教程中，学习如何将teb_local_planner设置为导航的本地规划程序插件。 4.避障和机器人轨迹模型 在本教程中，了解如何实现避障。描述了主要关注机器人轨迹模型及其影响的必要参数设置。 5.遵循全局规划（Via-Points） 在本教程中，学习如何配置局部规划程序以更严格地遵循全局规划。特别是，学习如何调整时间最优性和路径跟踪之间的权衡。 6.代价地图转换 在本教程中，学习如何应用costmap转换插件将已占用的costmap2d单元格转换为几何图元以进行优化（实验）。 7.规划类似汽车（ackermann）的机器人 在本教程中，学习如何为类似汽车的机器人设置计划器（实验）。 8.规划完整机器人 在本教程中，学习如何设置完整机器人的计划器（实验）。 9.考虑定制的障碍 在本教程中，学习如何将从其他节点发布的多边形障碍考虑在内。 10.考虑动态障碍 在本教程中，学习如何将从其他节点发布的动态障碍考虑在内。 11.通过costmap_converter跟踪并包含动态障碍物 在本教程中，学习如何利用代价地图转换器根据代价地图更新轻松跟踪动态障碍。 常见问题 此页面试图回答和解释有关teb_local_planner的常见问题。 此类机器人由于自身特性，路径规划并不连续，参考如下： 恢复行为尝试使用流清除空间，如下所示：
- 1.机器人卡住，禁止机器人向任何方向转弯。
- 2.退回空间，转向。
- 3. 在机器人退后的位置扫描导航的代价地图。
- 4.检测到最近障碍物的方向并尝试向相反方向转弯。
插件经常检查在恢复期间是否存在前进方向上的障碍物以保护机器人。在机器人的近点处检测障碍物使其制动并在制动点处调用局部计划。 
参考代码：https://github.com/CIR-KIT/steer_drive_ros
#include <stepback_and_steerturn_recovery/stepback_and_steerturn_recovery.h>
#include <pluginlib/class_list_macros.h>
#include <tf/transform_datatypes.h>
// register as a RecoveryBehavior plugin
PLUGINLIB_DECLARE_CLASS(stepback_and_steerturn_recovery, StepBackAndSteerTurnRecovery, stepback_and_steerturn_recovery::StepBackAndSteerTurnRecovery,
nav_core::RecoveryBehavior)
namespace stepback_and_steerturn_recovery
{
StepBackAndSteerTurnRecovery::StepBackAndSteerTurnRecovery () :
global_costmap_(NULL), local_costmap_(NULL), tf_(NULL), initialized_(false)
{
TWIST_STOP.linear.x = 0.0;
TWIST_STOP.linear.y = 0.0;
TWIST_STOP.linear.z = 0.0;
TWIST_STOP.angular.x = 0.0;
TWIST_STOP.angular.y = 0.0;
TWIST_STOP.angular.z = 0.0;
}
StepBackAndSteerTurnRecovery::~StepBackAndSteerTurnRecovery ()
{
delete world_model_;
}
void StepBackAndSteerTurnRecovery::initialize (std::string name, tf::TransformListener* tf,
cmap::Costmap2DROS* global_cmap, cmap::Costmap2DROS* local_cmap)
{
ROS_ASSERT(!initialized_);
name_ = name;
tf_ = tf;
local_costmap_ = local_cmap;
global_costmap_ = global_cmap;
/*
local_costmap_->getCostmapCopy(costmap_);
world_model_ = new blp::CostmapModel(costmap_);
*/
world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
cmd_vel_pub_ = nh_.advertise<gm::Twist>("cmd_vel", 10);
recover_run_pub_ = nh_.advertise<std_msgs::Bool>("recover_run", 10);
ros::NodeHandle private_nh("~/" + name);
/*
{
bool found=true;
found = found && private_nh.getParam("linear_x", base_frame_twist_.linear.x);
found = found && private_nh.getParam("linear_y", base_frame_twist_.linear.y);
found = found && private_nh.getParam("angular_z", base_frame_twist_.angular.z);
if (!found) {
ROS_FATAL_STREAM ("Didn't find twist parameters in " << private_nh.getNamespace());
ros::shutdown();
}
}
*/
private_nh.param("duration", duration_, 1.0);
private_nh.param("linear_speed_limit", linear_speed_limit_, 0.3);
private_nh.param("angular_speed_limit", angular_speed_limit_, 1.0);
private_nh.param("linear_acceleration_limit", linear_acceleration_limit_, 4.0);
private_nh.param("angular_acceleration_limit", angular_acceleration_limit_, 3.2);
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("simulation_frequency", simulation_frequency_, 5.0);
private_nh.param("simulation_inc", simulation_inc_, 1/simulation_frequency_);
private_nh.param("only_single_steering", only_single_steering_, true);
private_nh.param("trial_times", trial_times_, 5);
private_nh.param("obstacle_patience", obstacle_patience_, 0.5);
private_nh.param("obstacle_check_frequency", obstacle_check_frequency_, 5.0);
private_nh.param("sim_angle_resolution", sim_angle_resolution_, 0.1);
// back
private_nh.param("linear_vel_back", linear_vel_back_, -0.3);
private_nh.param("step_back_length", step_back_length_, 1.0);
private_nh.param("step_back_timeout", step_back_timeout_, 15.0);
//-- steer
private_nh.param("linear_vel_steer", linear_vel_steer_, 0.3);
private_nh.param("angular_speed_steer", angular_speed_steer_, 0.5);
private_nh.param("turn_angle", turn_angle_, 2.0);
private_nh.param("steering_timeout", steering_timeout_, 15.0);
//-- forward
private_nh.param("linear_vel_forward", linear_vel_forward_, 0.3);
private_nh.param("step_forward_length", step_forward_length_, 0.5);
private_nh.param("step_forward_timeout", step_forward_timeout_, 15.0);
/*
ROS_INFO_STREAM_NAMED ("top", "Initialized twist recovery with twist " <<
base_frame_twist_ << " and duration " << duration_);
*/
ROS_INFO_NAMED ("top", "Initialized with only_single_steering = %s", (only_single_steering_ ? "true" : "false"));
ROS_INFO_NAMED ("top", "Initialized with recovery_trial_times = %d", trial_times_);
ROS_INFO_NAMED ("top", "Initialized with obstacle_patience = %.2f", obstacle_patience_);
ROS_INFO_NAMED ("top", "Initialized with obstacle_check_frequency = %.2f", obstacle_check_frequency_);
ROS_INFO_NAMED ("top", "Initialized with simulation_frequency = %.2f", simulation_frequency_);
ROS_INFO_NAMED ("top", "Initialized with sim_angle_resolution = %.2f", sim_angle_resolution_);
ROS_INFO_NAMED ("top", "Initialized with linear_vel_back = %.2f, step_back_length = %.2f, step_back_steering = %.2f",
linear_vel_back_, step_back_length_, step_back_timeout_);
ROS_INFO_NAMED ("top", "Initialized with linear_vel_steer = %.2f, angular_speed_steer = %.2f,"
" turn_angle = %.2f, steering_timeout = %.2f",
linear_vel_steer_, angular_speed_steer_, turn_angle_, steering_timeout_);
ROS_INFO_NAMED ("top", "Initialized with linear_vel_forward = %.2f, step_forward_length = %.2f, step_forward_timeout = %.2f",
linear_vel_forward_, step_forward_length_, step_forward_timeout_);
initialized_ = true;
}
gm::Twist scaleTwist (const gm::Twist& twist, const double scale)
{
gm::Twist t;
t.linear.x = twist.linear.x * scale;
t.linear.y = twist.linear.y * scale;
t.angular.z = twist.angular.z * scale;
return t;
}
gm::Pose2D forwardSimulate (const gm::Pose2D& p, const gm::Twist& twist, const double t=1.0)
{
gm::Pose2D p2;
const double linear_vel = twist.linear.x;
p2.theta = p.theta + twist.angular.z;//*t;
p2.x = p.x + linear_vel * cos(p2.theta)*t;
p2.y = p.y + linear_vel * sin(p2.theta)*t;
return p2;
}
/// Return the cost of a pose, modified so that -1 does not equal infinity; instead 1e9 does.
double StepBackAndSteerTurnRecovery::normalizedPoseCost (const gm::Pose2D& pose) const
{
gm::Point p;
p.x = pose.x;
p.y = pose.y;
unsigned int pose_map_idx_x, pose_map_idx_y;
costmap_2d::Costmap2D* costmap = local_costmap_->getCostmap();
costmap->worldToMap(p.x, p.y, pose_map_idx_x, pose_map_idx_y); // convert point unit from [m] to [idx]
ROS_DEBUG_NAMED ("top", "Trying to get cost at (%d, %d) in getCost", pose_map_idx_x, pose_map_idx_y);
const double c = costmap->getCost(pose_map_idx_x, pose_map_idx_y);
return c < 0 ? 1e9 : c;
}
/// Return the maximum d <= duration_ such that starting at the current pose, the cost is nonincreasing for
/// d seconds if we follow twist
/// It might also be good to have a threshold such that we're allowed to have lethal cost for at most
/// the first k of those d seconds, but this is not done
gm::Pose2D StepBackAndSteerTurnRecovery::getPoseToObstacle (const gm::Pose2D& current, const gm::Twist& twist) const
{
double cost = 0;
cost = normalizedPoseCost(current);
double t; // Will hold the first time that is invalid
gm::Pose2D current_tmp = current;
double next_cost;
ROS_DEBUG_NAMED ("top", " ");
for (t=simulation_inc_; t<=duration_ + 500; t+=simulation_inc_) {
ROS_DEBUG_NAMED ("top", "start loop");
current_tmp = forwardSimulate(current, twist, t);
ROS_DEBUG_NAMED ("top", "finish fowardSimulate");
next_cost = normalizedPoseCost(current_tmp);
ROS_DEBUG_NAMED ("top", "finish Cost");
//if (next_cost > cost) {
if (/*next_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||*/ next_cost == costmap_2d::LETHAL_OBSTACLE) {
ROS_DEBUG_STREAM_NAMED ("cost", "Cost at " << t << " and pose " << forwardSimulate(current, twist, t)
<< " is " << next_cost << " which is greater than previous cost " << cost);
break;
}
cost = next_cost;
}
ROS_DEBUG_NAMED ("top", "cost = %.2f, next_cost = %.2f", cost, next_cost);
ROS_DEBUG_NAMED ("top", "twist.linear.x = %.2f, twist.angular.z = %.2f", twist.linear.x, twist.angular.z);
ROS_DEBUG_NAMED ("top", "init = (%.2f, %.2f, %.2f), current = (%.2f, %.2f, %.2f)",
current.x, current.y, current.theta, current_tmp.x, current_tmp.y, current_tmp.theta);
ROS_DEBUG_NAMED ("top", "time = %.2f", t);
// return t-simulation_inc_;
return current_tmp;
}
double linearSpeed (const gm::Twist& twist)
{
return sqrt(twist.linear.x*twist.linear.x + twist.linear.y*twist.linear.y);
}
double angularSpeed (const gm::Twist& twist)
{
return fabs(twist.angular.z);
}
// Scale twist so we can stop in the given time, and so it's within the max velocity
gm::Twist StepBackAndSteerTurnRecovery::scaleGivenAccelerationLimits (const gm::Twist& twist, const double time_remaining) const
{
const double linear_speed = linearSpeed(twist);
const double angular_speed = angularSpeed(twist);
const double linear_acc_scaling = linear_speed/(time_remaining*linear_acceleration_limit_);
const double angular_acc_scaling = angular_speed/(time_remaining*angular_acceleration_limit_);
const double acc_scaling = max(linear_acc_scaling, angular_acc_scaling);
const double linear_vel_scaling = linear_speed/linear_speed_limit_;
const double angular_vel_scaling = angular_speed/angular_speed_limit_;
const double vel_scaling = max(linear_vel_scaling, angular_vel_scaling);
return scaleTwist(twist, max(1.0, max(acc_scaling, vel_scaling)));
}
// Get pose in local costmap framoe
gm::Pose2D StepBackAndSteerTurnRecovery::getCurrentLocalPose () const
{
tf::Stamped<tf::Pose> p;
local_costmap_->getRobotPose(p);
gm::Pose2D pose;
pose.x = p.getOrigin().x();
pose.y = p.getOrigin().y();
pose.theta = tf::getYaw(p.getRotation());
return pose;
}
void StepBackAndSteerTurnRecovery::moveSpacifiedLength (const gm::Twist twist, const double distination, const COSTMAP_SEARCH_MODE mode) const
{
double distination_cmd = distination;
double min_dist_to_obstacle = getMinimalDistanceToObstacle(mode);
std::string mode_name;
double time_out;
switch (mode) {
case FORWARD:
mode_name = "FORWARD";
time_out = step_forward_timeout_;
if (min_dist_to_obstacle < distination)
{
distination_cmd = min_dist_to_obstacle - obstacle_patience_;
ROS_WARN_NAMED ("top", "obstacle detected before moving %s", mode_name.c_str());
ROS_WARN_NAMED ("top", "min dist to obstacle = %.2f [m] in %s", min_dist_to_obstacle, mode_name.c_str());
ROS_WARN_NAMED ("top", "moving length is switched from %.2f [m] to %.2f in %s", distination, distination_cmd,mode_name.c_str());
}
break;
case FORWARD_LEFT:
mode_name = "FORWARD_LEFT";
time_out = steering_timeout_;
if (min_dist_to_obstacle < obstacle_patience_)
{
distination_cmd = 0.0;
ROS_WARN_NAMED ("top", "obstacle detected before moving %s", mode_name.c_str());
ROS_WARN_NAMED ("top", "min dist to obstacle = %.2f [m] in %s", min_dist_to_obstacle, mode_name.c_str());
ROS_WARN_NAMED ("top", "stop turning because an obstacle is too close in %s", mode_name.c_str());
}
break;
case FORWARD_RIGHT:
mode_name = "FORWARD_RIGHT";
time_out = steering_timeout_;
if (min_dist_to_obstacle < obstacle_patience_)
{
distination_cmd = 0.0;
ROS_WARN_NAMED ("top", "obstacle detected before moving %s", mode_name.c_str());
ROS_WARN_NAMED ("top", "min dist to obstacle = %.2f [m] in %s", min_dist_to_obstacle, mode_name.c_str());
ROS_WARN_NAMED ("top", "stop turning because an obstacle is too close in %s", mode_name.c_str());
}
break;
case BACKWARD:
mode_name = "BACKWARD";
time_out = step_back_timeout_;
if (min_dist_to_obstacle < distination)
{
distination_cmd = min_dist_to_obstacle - 2 * obstacle_patience_;
ROS_WARN_NAMED ("top", "obstacle detected before moving %s", mode_name.c_str());
ROS_WARN_NAMED ("top", "min dist to obstacle = %.2f [m] in %s", min_dist_to_obstacle, mode_name.c_str());
ROS_WARN_NAMED ("top", "moving length is switched from %.2f [m] to %.2f in %s", distination, distination_cmd, mode_name.c_str());
}
break;
default:
break;
}
const double frequency = 5.0;
ros::Rate r(frequency);
const gm::Pose2D initialPose = getCurrentLocalPose();
int log_cnt = 0;
int log_frequency = (int)obstacle_check_frequency_;
ros::Time time_begin = ros::Time::now();
while (double dist_diff = getCurrentDistDiff(initialPose, distination_cmd, mode) > 0.01)
{
double remaining_time = dist_diff / base_frame_twist_.linear.x;
double min_dist = getMinimalDistanceToObstacle(mode);
// time out
if(time_out > 0.0 &&
time_begin + ros::Duration(time_out) < ros::Time::now())
{
//cmd_vel_pub_.publish(scaleGivenAccelerationLimits(TWIST_STOP, remaining_time));
cmd_vel_pub_.publish(TWIST_STOP);
ROS_WARN_NAMED ("top", "time out at %s", mode_name.c_str());
ROS_WARN_NAMED ("top", "%.2f [sec] elapsed.", time_out);
break;
}
// detect an obstacle
if(min_dist < obstacle_patience_)
{
//cmd_vel_pub_.publish(scaleGivenAccelerationLimits(TWIST_STOP, remaining_time));
cmd_vel_pub_.publish(TWIST_STOP);
ROS_WARN_NAMED ("top", "obstacle detected at %s", mode_name.c_str());
ROS_WARN_NAMED ("top", "min dist to obstacle = %.2f [m] in %s", min_dist, mode_name.c_str());
break;
}
//cmd_vel_pub_.publish(scaleGivenAccelerationLimits(twist, remaining_time));
cmd_vel_pub_.publish(twist);
if(log_cnt++ % log_frequency == 0)
{
ROS_DEBUG_NAMED ("top", "no obstacle around");
ROS_INFO_NAMED ("top", "min dist to obstacle = %.2f [m] in %s", min_dist, mode_name.c_str());
}
ros::spinOnce();
r.sleep();
}
return;
}
double StepBackAndSteerTurnRecovery::getCurrentDiff(const gm::Pose2D initialPose, const COSTMAP_SEARCH_MODE mode) const
{
const gm::Pose2D& currentPose = getCurrentLocalPose();
ROS_DEBUG_NAMED ("top", "current pose (%.2f, %.2f, %.2f)", currentPose.x,
currentPose.y, currentPose.theta);
double current_diff;
switch (mode) {
case FORWARD:
case BACKWARD:
current_diff = getDistBetweenTwoPoints(currentPose, initialPose);
ROS_DEBUG_NAMED ("top", "current_diff in translation = %.2f", current_diff);
break;
case FORWARD_LEFT:
case FORWARD_RIGHT:
current_diff = initialPose.theta - currentPose.theta;
current_diff = fabs(current_diff);
ROS_DEBUG_NAMED ("top", "initialPose.Theta = %.2f, currentPose.theta = %.2f", initialPose.theta, currentPose.theta);
ROS_DEBUG_NAMED ("top", "current_diff in angle = %.2f", current_diff);
default:
break;
}
return current_diff;
}
double StepBackAndSteerTurnRecovery::getCurrentDistDiff(const gm::Pose2D initialPose, const double distination, COSTMAP_SEARCH_MODE mode) const
{
const double dist_diff = distination - getCurrentDiff(initialPose, mode);
ROS_DEBUG_NAMED ("top", "dist_diff = %.2f", dist_diff);
return dist_diff;
}
double StepBackAndSteerTurnRecovery::getMinimalDistanceToObstacle(const COSTMAP_SEARCH_MODE mode) const
{
double max_angle, min_angle;
gm::Twist twist = TWIST_STOP;
switch (mode) {
case FORWARD:
twist.linear.x = linear_vel_forward_;
max_angle = M_PI/3.0;
min_angle = -M_PI/3.0;
break;
case FORWARD_LEFT:
twist.linear.x = linear_vel_forward_;
max_angle = M_PI/2.0;
min_angle = 0.0;
break;
case FORWARD_RIGHT:
twist.linear.x = linear_vel_forward_;
max_angle = 0.0;
min_angle = -M_PI/2.0;
break;
case BACKWARD:
twist.linear.x = linear_vel_back_;
max_angle = M_PI/3.0;
min_angle = -M_PI/3.0;
break;
default:
break;
}
const gm::Pose2D& current = getCurrentLocalPose();
double min_dist = INFINITY;
for(double angle = min_angle; angle < max_angle; angle+=sim_angle_resolution_)
{
twist.angular.z = angle;
gm::Pose2D pose_to_obstacle = getPoseToObstacle(current, twist);
double dist_to_obstacle = getDistBetweenTwoPoints(current, pose_to_obstacle);
if(dist_to_obstacle < min_dist)
min_dist = dist_to_obstacle;
}
ROS_DEBUG_NAMED ("top", "min_dist = %.2f", min_dist);
return min_dist;
}
int StepBackAndSteerTurnRecovery::determineTurnDirection()
{
// simulate and evaluate cost
const gm::Pose2D& current = getCurrentLocalPose();
gm::Twist twist = TWIST_STOP;
twist.linear.x = linear_vel_forward_;
vector<double> dist_to_obstacle_r;
vector<double> dist_to_obstacle_l;
double max = M_PI/2.0;
double min = - max;
for(double angle = min; angle < max; angle+=sim_angle_resolution_)
{
twist.angular.z = angle;
gm::Pose2D pose_to_obstacle = getPoseToObstacle(current, twist);
double dist_to_obstacle = getDistBetweenTwoPoints(current, pose_to_obstacle);
ROS_DEBUG_NAMED ("top", "(%.2f, %.2f, %.2f) for %.2f [m] to obstacle",
twist.linear.x, twist.linear.y, twist.angular.z, dist_to_obstacle);
if(angle > 0.0)
dist_to_obstacle_l.push_back(dist_to_obstacle);
else if(angle < 0.0)
dist_to_obstacle_r.push_back(dist_to_obstacle);
else
;// do nothing
}
// determine the directoin to go from cost
/*
double sum_l = 0.0;
double sum_r = 0.0;
double ave_l = 0.0;
double ave_r = 0.0;
for(int i = 0; i < dist_to_obstacle_l.size(); i++)
sum_l += dist_to_obstacle_l[i];
for(int i = 0; i < dist_to_obstacle_r.size(); i++)
sum_r += dist_to_obstacle_r[i];
ave_l = sum_l / dist_to_obstacle_l.size();
ave_r = sum_r / dist_to_obstacle_r.size();
ROS_DEBUG_NAMED ("top", "sum_l = %.2f, sum_r = %.2f", sum_l, sum_r);
ROS_DEBUG_NAMED ("top", "size_l = %d, size_r = %d", (int)dist_to_obstacle_l.size(), (int)dist_to_obstacle_r.size());
ROS_DEBUG_NAMED ("top", "ave_l = %.2f, ave_r = %.2f", ave_l, ave_r);
*/
double min_l = *min_element(dist_to_obstacle_l.begin(), dist_to_obstacle_l.end());
double min_r = *min_element(dist_to_obstacle_r.begin(), dist_to_obstacle_r.end());
ROS_INFO_NAMED ("top", "min_l = %.2f [m], min_r = %.2f [m]", min_l, min_r);
int ret_val;
if(min_l < min_r)
ret_val = RIGHT; // if obstacle settles on left, turn right
else
ret_val = LEFT; // vice versa
return ret_val;
}
double StepBackAndSteerTurnRecovery::getDistBetweenTwoPoints(const gm::Pose2D pose1, const gm::Pose2D pose2) const
{
double dist_to_obstacle = (pose1.x - pose2.x) * (pose1.x - pose2.x) +
(pose1.y - pose2.y) * (pose1.y - pose2.y);
return sqrt(dist_to_obstacle);
}
void StepBackAndSteerTurnRecovery::runBehavior ()
{
ROS_ASSERT (initialized_);
ROS_INFO_NAMED ("top", "*****************************************************");
ROS_INFO_NAMED ("top", "********Start StepBackAndSteerTurnRecovery!!!********");
ROS_INFO_NAMED ("top", "*****************************************************");
std_msgs::Bool run_state;
// when starting recovery, topic /run_state_ shifts to true
run_state.data = true;
recover_run_pub_.publish(run_state);
int cnt = 0;
const double stop_duaration = 1.0;
while(true)
{
cnt++;
ROS_INFO_NAMED ("top", "==== %d th recovery trial ====", cnt);
// Figure out how long we can safely run the behavior
const gm::Pose2D& initialPose = getCurrentLocalPose();
// initial pose
ROS_DEBUG_NAMED ("top", "initial pose (%.2f, %.2f, %.2f)", initialPose.x,
initialPose.y, initialPose.theta);
ros::Rate r(controller_frequency_);
// step back
base_frame_twist_.linear.x = linear_vel_back_;
ROS_INFO_NAMED ("top", "attempting step back");
moveSpacifiedLength(base_frame_twist_, step_back_length_, BACKWARD);
ROS_INFO_NAMED ("top", "complete step back");
double final_diff = getCurrentDiff(initialPose);
ROS_DEBUG_NAMED ("top", "final_diff = %.2f",final_diff);
// stop
for (double t=0; t<stop_duaration; t+=1/controller_frequency_) {
cmd_vel_pub_.publish(TWIST_STOP);
r.sleep();
}
int turn_dir = determineTurnDirection();
int costmap_search_mode[CNT_TURN];
double z;
if(turn_dir == LEFT)
{
z = angular_speed_steer_;
costmap_search_mode[FIRST_TURN] = FORWARD_LEFT;
costmap_search_mode[SECOND_TURN] = FORWARD_RIGHT;
ROS_INFO_NAMED ("top", "attempting to turn left at the 1st turn");
}
else
{
z = -1 * angular_speed_steer_;
costmap_search_mode[FIRST_TURN] = FORWARD_RIGHT;
costmap_search_mode[SECOND_TURN] = FORWARD_LEFT;
ROS_INFO_NAMED ("top", "attemping to turn right at the 1st turn");
}
// clear way
//-- first steering
gm::Twist twist;
twist = TWIST_STOP;
twist.linear.x = linear_vel_steer_;
twist.angular.z = z;
moveSpacifiedLength(twist, turn_angle_, (COSTMAP_SEARCH_MODE)costmap_search_mode[FIRST_TURN]);
ROS_INFO_NAMED ("top", "complete the 1st turn");
if(!only_single_steering_) {
//-- go straight
ROS_INFO_NAMED ("top", "attemping step forward");
twist = TWIST_STOP;
twist.linear.x = linear_vel_forward_;
moveSpacifiedLength(twist, step_forward_length_, FORWARD);
ROS_INFO_NAMED ("top", "complete step forward");
//-- second steering
ROS_INFO_NAMED ("top", "attempting second turn");
twist = TWIST_STOP;
twist.linear.x = linear_vel_steer_;
twist.angular.z = -z;
moveSpacifiedLength(twist, turn_angle_, (COSTMAP_SEARCH_MODE)costmap_search_mode[SECOND_TURN]);
ROS_INFO_NAMED ("top", "complete second turn");
}
// stop
for (double t=0; t<stop_duaration; t+=1/controller_frequency_) {
cmd_vel_pub_.publish(TWIST_STOP);
r.sleep();
}
// check trial times
if(cnt == trial_times_)
{
ROS_INFO_NAMED ("top", "break after %d times recovery", cnt);
break;
}
// check clearance forward
const gm::Pose2D& current = getCurrentLocalPose();
double max_angle = 0.1;
double min_angle = -max_angle;
double max_clearance = 0;
twist.linear.x = 3.0;
for(double angle = min_angle; angle < max_angle; angle+=sim_angle_resolution_)
{
twist.angular.z = angle;
gm::Pose2D pose_to_obstacle = getPoseToObstacle(current, twist);
double dist_to_obstacle = getDistBetweenTwoPoints(current, pose_to_obstacle);
if(dist_to_obstacle > max_clearance)
max_clearance = dist_to_obstacle;
}
if(max_clearance < 3.0)
{
ROS_INFO_NAMED ("top", "continue recovery because the robot couldn't get clearance");
ROS_DEBUG_NAMED ("top", "continue at (%.2f, %.2f, %.2f) for max_clearance %.2f m",
twist.linear.x, twist.linear.y, twist.angular.z, max_clearance);
continue;
}
else
{
ROS_INFO_NAMED ("top", "break recovery because the robot got clearance");
ROS_DEBUG_NAMED ("top", "break at (%.2f, %.2f, %.2f) for max_clearance %.2f m",
twist.linear.x, twist.linear.y, twist.angular.z, max_clearance);
break;
}
}
// when finishing recovery, topic /run_state_ shifts to false
run_state.data = false;
recover_run_pub_.publish(run_state);
ROS_INFO_NAMED ("top", "*****************************************************");
ROS_INFO_NAMED ("top", "********Finish StepBackAndSteerTurnRecovery!!********");
ROS_INFO_NAMED ("top", "*****************************************************");
}
} // namespace stepback_and_steerturn_recovery其他相关资料参考：
推荐tianbot的racecar的github，供参考学习：https://github.com/tianbot/tianbot_racecar L1_controller_v2.cpp
#include <iostream>
#include "ros/ros.h"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include "nav_msgs/Path.h"
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#define PI 3.14159265358979
/********************/
/* CLASS DEFINITION */
/********************/
class L1Controller
{
public:
L1Controller();
void initMarker();
bool isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose);
bool isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos);
double getYawFromPose(const geometry_msgs::Pose& carPose);
double getEta(const geometry_msgs::Pose& carPose);
double getCar2GoalDist();
double getL1Distance(const double& _Vcmd);
double getSteeringAngle(double eta);
double getGasInput(const float& current_v);
geometry_msgs::Point get_odom_car2WayPtVec(const geometry_msgs::Pose& carPose);
private:
ros::NodeHandle n_;
ros::Subscriber odom_sub, path_sub, goal_sub;
ros::Publisher pub_, marker_pub;
ros::Timer timer1, timer2;
tf::TransformListener tf_listener;
visualization_msgs::Marker points, line_strip, goal_circle;
geometry_msgs::Twist cmd_vel;
geometry_msgs::Point odom_goal_pos;
nav_msgs::Odometry odom;
nav_msgs::Path map_path, odom_path;
double L, Lfw, Lrv, Vcmd, lfw, lrv, steering, u, v;
double Gas_gain, baseAngle, Angle_gain, goalRadius;
int controller_freq, baseSpeed;
bool foundForwardPt, goal_received, goal_reached;
void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);
void pathCB(const nav_msgs::Path::ConstPtr& pathMsg);
void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg);
void goalReachingCB(const ros::TimerEvent&);
void controlLoopCB(const ros::TimerEvent&);
}; // end of class
L1Controller::L1Controller()
{
//Private parameters handler
ros::NodeHandle pn("~");
//Car parameter
pn.param("L", L, 0.26);
pn.param("Lrv", Lrv, 10.0);
pn.param("Vcmd", Vcmd, 1.0);
pn.param("lfw", lfw, 0.13);
pn.param("lrv", lrv, 10.0);
//Controller parameter
pn.param("controller_freq", controller_freq, 20);
pn.param("AngleGain", Angle_gain, -1.0);
pn.param("GasGain", Gas_gain, 1.0);
pn.param("baseSpeed", baseSpeed, 1470);
pn.param("baseAngle", baseAngle, 90.0);
//Publishers and Subscribers
odom_sub = n_.subscribe("/odometry/filtered", 1, &L1Controller::odomCB, this);
path_sub = n_.subscribe("/move_base_node/NavfnROS/plan", 1, &L1Controller::pathCB, this);
goal_sub = n_.subscribe("/move_base_simple/goal", 1, &L1Controller::goalCB, this);
marker_pub = n_.advertise<visualization_msgs::Marker>("car_path", 10);
pub_ = n_.advertise<geometry_msgs::Twist>("car/cmd_vel", 1);
//Timer
timer1 = n_.createTimer(ros::Duration((1.0)/controller_freq), &L1Controller::controlLoopCB, this); // Duration(0.05) -> 20Hz
timer2 = n_.createTimer(ros::Duration((0.5)/controller_freq), &L1Controller::goalReachingCB, this); // Duration(0.05) -> 20Hz
//Init variables
Lfw = goalRadius = getL1Distance(Vcmd);
foundForwardPt = false;
goal_received = false;
goal_reached = false;
cmd_vel.linear.x = 1500; // 1500 for stop
cmd_vel.angular.z = baseAngle;
//Show info
ROS_INFO("[param] baseSpeed: %d", baseSpeed);
ROS_INFO("[param] baseAngle: %f", baseAngle);
ROS_INFO("[param] AngleGain: %f", Angle_gain);
ROS_INFO("[param] Vcmd: %f", Vcmd);
ROS_INFO("[param] Lfw: %f", Lfw);
//Visualization Marker Settings
initMarker();
}
void L1Controller::initMarker()
{
points.header.frame_id = line_strip.header.frame_id = goal_circle.header.frame_id = "odom";
points.ns = line_strip.ns = goal_circle.ns = "Markers";
points.action = line_strip.action = goal_circle.action = visualization_msgs::Marker::ADD;
points.pose.orientation.w = line_strip.pose.orientation.w = goal_circle.pose.orientation.w = 1.0;
points.id = 0;
line_strip.id = 1;
goal_circle.id = 2;
points.type = visualization_msgs::Marker::POINTS;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
goal_circle.type = visualization_msgs::Marker::CYLINDER;
// POINTS markers use x and y scale for width/height respectively
points.scale.x = 0.2;
points.scale.y = 0.2;
//LINE_STRIP markers use only the x component of scale, for the line width
line_strip.scale.x = 0.1;
goal_circle.scale.x = goalRadius;
goal_circle.scale.y = goalRadius;
goal_circle.scale.z = 0.1;
// Points are green
points.color.g = 1.0f;
points.color.a = 1.0;
// Line strip is blue
line_strip.color.b = 1.0;
line_strip.color.a = 1.0;
//goal_circle is yellow
goal_circle.color.r = 1.0;
goal_circle.color.g = 1.0;
goal_circle.color.b = 0.0;
goal_circle.color.a = 0.5;
}
void L1Controller::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
odom = *odomMsg;
}
void L1Controller::pathCB(const nav_msgs::Path::ConstPtr& pathMsg)
{
map_path = *pathMsg;
}
void L1Controller::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg)
{
try
{
geometry_msgs::PoseStamped odom_goal;
tf_listener.transformPose("odom", ros::Time(0) , *goalMsg, "map" ,odom_goal);
odom_goal_pos = odom_goal.pose.position;
goal_received = true;
goal_reached = false;
/*Draw Goal on RVIZ*/
goal_circle.pose = odom_goal.pose;
marker_pub.publish(goal_circle);
}
catch(tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
}
double L1Controller::getYawFromPose(const geometry_msgs::Pose& carPose)
{
float x = carPose.orientation.x;
float y = carPose.orientation.y;
float z = carPose.orientation.z;
float w = carPose.orientation.w;
double tmp,yaw;
tf::Quaternion q(x,y,z,w);
tf::Matrix3x3 quaternion(q);
quaternion.getRPY(tmp,tmp, yaw);
return yaw;
}
bool L1Controller::isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose)
{
float car2wayPt_x = wayPt.x - carPose.position.x;
float car2wayPt_y = wayPt.y - carPose.position.y;
double car_theta = getYawFromPose(carPose);
float car_car2wayPt_x = cos(car_theta)*car2wayPt_x + sin(car_theta)*car2wayPt_y;
float car_car2wayPt_y = -sin(car_theta)*car2wayPt_x + cos(car_theta)*car2wayPt_y;
if(car_car2wayPt_x >0) /*is Forward WayPt*/
return true;
else
return false;
}
bool L1Controller::isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos)
{
double dx = wayPt.x - car_pos.x;
double dy = wayPt.y - car_pos.y;
double dist = sqrt(dx*dx + dy*dy);
if(dist < Lfw)
return false;
else if(dist >= Lfw)
return true;
}
geometry_msgs::Point L1Controller::get_odom_car2WayPtVec(const geometry_msgs::Pose& carPose)
{
geometry_msgs::Point carPose_pos = carPose.position;
double carPose_yaw = getYawFromPose(carPose);
geometry_msgs::Point forwardPt;
geometry_msgs::Point odom_car2WayPtVec;
foundForwardPt = false;
if(!goal_reached){
for(int i =0; i< map_path.poses.size(); i++)
{
geometry_msgs::PoseStamped map_path_pose = map_path.poses[i];
geometry_msgs::PoseStamped odom_path_pose;
try
{
tf_listener.transformPose("odom", ros::Time(0) , map_path_pose, "map" ,odom_path_pose);
geometry_msgs::Point odom_path_wayPt = odom_path_pose.pose.position;
bool _isForwardWayPt = isForwardWayPt(odom_path_wayPt,carPose);
if(_isForwardWayPt)
{
bool _isWayPtAwayFromLfwDist = isWayPtAwayFromLfwDist(odom_path_wayPt,carPose_pos);
if(_isWayPtAwayFromLfwDist)
{
forwardPt = odom_path_wayPt;
foundForwardPt = true;
break;
}
}
}
catch(tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
}
}
else if(goal_reached)
{
forwardPt = odom_goal_pos;
foundForwardPt = false;
//ROS_INFO("goal REACHED!");
}
/*Visualized Target Point on RVIZ*/
/*Clear former target point Marker*/
points.points.clear();
line_strip.points.clear();
if(foundForwardPt && !goal_reached)
{
points.points.push_back(carPose_pos);
points.points.push_back(forwardPt);
line_strip.points.push_back(carPose_pos);
line_strip.points.push_back(forwardPt);
}
marker_pub.publish(points);
marker_pub.publish(line_strip);
odom_car2WayPtVec.x = cos(carPose_yaw)*(forwardPt.x - carPose_pos.x) + sin(carPose_yaw)*(forwardPt.y - carPose_pos.y);
odom_car2WayPtVec.y = -sin(carPose_yaw)*(forwardPt.x - carPose_pos.x) + cos(carPose_yaw)*(forwardPt.y - carPose_pos.y);
return odom_car2WayPtVec;
}
double L1Controller::getEta(const geometry_msgs::Pose& carPose)
{
geometry_msgs::Point odom_car2WayPtVec = get_odom_car2WayPtVec(carPose);
double eta = atan2(odom_car2WayPtVec.y,odom_car2WayPtVec.x);
return eta;
}
double L1Controller::getCar2GoalDist()
{
geometry_msgs::Point car_pose = odom.pose.pose.position;
double car2goal_x = odom_goal_pos.x - car_pose.x;
double car2goal_y = odom_goal_pos.y - car_pose.y;
double dist2goal = sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y);
return dist2goal;
}
double L1Controller::getL1Distance(const double& _Vcmd)
{
double L1 = 0;
if(_Vcmd < 1.34)
L1 = 3 / 3.0;
else if(_Vcmd > 1.34 && _Vcmd < 5.36)
L1 = _Vcmd*2.24 / 3.0;
else
L1 = 12 / 3.0;
return L1;
}
double L1Controller::getSteeringAngle(double eta)
{
double steeringAnge = -atan2((L*sin(eta)),(Lfw/2+lfw*cos(eta)))*(180.0/PI);
//ROS_INFO("Steering Angle = %.2f", steeringAnge);
return steeringAnge;
}
double L1Controller::getGasInput(const float& current_v)
{
double u = (Vcmd - current_v)*Gas_gain;
//ROS_INFO("velocity = %.2f\tu = %.2f",current_v, u);
return u;
}
void L1Controller::goalReachingCB(const ros::TimerEvent&)
{
if(goal_received)
{
double car2goal_dist = getCar2GoalDist();
if(car2goal_dist < goalRadius)
{
goal_reached = true;
goal_received = false;
ROS_INFO("Goal Reached !");
}
}
}
void L1Controller::controlLoopCB(const ros::TimerEvent&)
{
geometry_msgs::Pose carPose = odom.pose.pose;
geometry_msgs::Twist carVel = odom.twist.twist;
cmd_vel.linear.x = 1500;
cmd_vel.angular.z = baseAngle;
if(goal_received)
{
/*Estimate Steering Angle*/
double eta = getEta(carPose);
if(foundForwardPt)
{
cmd_vel.angular.z = baseAngle + getSteeringAngle(eta)*Angle_gain;
/*Estimate Gas Input*/
if(!goal_reached)
{
//double u = getGasInput(carVel.linear.x);
//cmd_vel.linear.x = baseSpeed - u;
cmd_vel.linear.x = baseSpeed;
ROS_DEBUG("\nGas = %.2f\nSteering angle = %.2f",cmd_vel.linear.x,cmd_vel.angular.z);
}
}
}
pub_.publish(cmd_vel);
}
/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{
//Initiate ROS
ros::init(argc, argv, "L1Controller_v2");
L1Controller controller;
ros::spin();
return 0;
}也可以参考minicar的MPC和PurePursuit
#include <iostream>
#include <map>
#include <math.h>
#include "ros/ros.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <visualization_msgs/Marker.h>
#include "MPC.h"
#include <Eigen/Core>
#include <Eigen/QR>
using namespace std;
using namespace Eigen;
/********************/
/* CLASS DEFINITION */
/********************/
class MPCNode
{
public:
MPCNode();
int get_thread_numbers();
private:
ros::NodeHandle _nh;
ros::Subscriber _sub_odom, _sub_path, _sub_goal, _sub_amcl;
ros::Publisher _pub_odompath, _pub_twist, _pub_ackermann, _pub_mpctraj;
ros::Timer _timer1;
tf::TransformListener _tf_listener;
geometry_msgs::Point _goal_pos;
nav_msgs::Odometry _odom;
nav_msgs::Path _odom_path, _mpc_traj;
ackermann_msgs::AckermannDriveStamped _ackermann_msg;
geometry_msgs::Twist _twist_msg;
string _globalPath_topic, _goal_topic;
string _map_frame, _odom_frame, _car_frame;
MPC _mpc;
map<string, double> _mpc_params;
double _mpc_steps, _ref_cte, _ref_epsi, _ref_vel, _w_cte, _w_epsi, _w_vel,
_w_delta, _w_accel, _w_delta_d, _w_accel_d, _max_steering, _max_throttle, _bound_value;
double _Lf, _dt, _steering, _throttle, _speed, _max_speed;
double _pathLength, _goalRadius, _waypointsDist;
int _controller_freq, _downSampling, _thread_numbers;
bool _goal_received, _goal_reached, _path_computed, _pub_twist_flag, _debug_info, _delay_mode;
double polyeval(Eigen::VectorXd coeffs, double x);
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);
void pathCB(const nav_msgs::Path::ConstPtr& pathMsg);
void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg);
void amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg);
void controlLoopCB(const ros::TimerEvent&);
}; // end of class
MPCNode::MPCNode()
{
//Private parameters handler
ros::NodeHandle pn("~");
//Parameters for control loop
pn.param("thread_numbers", _thread_numbers, 2); // number of threads for this ROS node
pn.param("pub_twist_cmd", _pub_twist_flag, true);
pn.param("debug_info", _debug_info, false);
pn.param("delay_mode", _delay_mode, true);
pn.param("max_speed", _max_speed, 2.0); // unit: m/s
pn.param("waypoints_dist", _waypointsDist, -1.0); // unit: m
pn.param("path_length", _pathLength, 8.0); // unit: m
pn.param("goal_radius", _goalRadius, 0.5); // unit: m
pn.param("controller_freq", _controller_freq, 10);
pn.param("vehicle_Lf", _Lf, 0.25); // distance between the front of the vehicle and its center of gravity
_dt = double(1.0/_controller_freq); // time step duration dt in s
//Parameter for MPC solver
pn.param("mpc_steps", _mpc_steps, 20.0);
pn.param("mpc_ref_cte", _ref_cte, 0.0);
pn.param("mpc_ref_epsi", _ref_epsi, 0.0);
pn.param("mpc_ref_vel", _ref_vel, 1.5);
pn.param("mpc_w_cte", _w_cte, 100.0);
pn.param("mpc_w_epsi", _w_epsi, 100.0);
pn.param("mpc_w_vel", _w_vel, 100.0);
pn.param("mpc_w_delta", _w_delta, 100.0);
pn.param("mpc_w_accel", _w_accel, 50.0);
pn.param("mpc_w_delta_d", _w_delta_d, 0.0);
pn.param("mpc_w_accel_d", _w_accel_d, 0.0);
pn.param("mpc_max_steering", _max_steering, 0.523); // Maximal steering radian (~30 deg)
pn.param("mpc_max_throttle", _max_throttle, 1.0); // Maximal throttle accel
pn.param("mpc_bound_value", _bound_value, 1.0e3); // Bound value for other variables
//Parameter for topics & Frame name
pn.param<std::string>("global_path_topic", _globalPath_topic, "/move_base/TrajectoryPlannerROS/global_plan" );
pn.param<std::string>("goal_topic", _goal_topic, "/move_base_simple/goal" );
pn.param<std::string>("map_frame", _map_frame, "map" );
pn.param<std::string>("odom_frame", _odom_frame, "odom");
pn.param<std::string>("car_frame", _car_frame, "base_link" );
//Display the parameters
cout << "\n===== Parameters =====" << endl;
cout << "pub_twist_cmd: " << _pub_twist_flag << endl;
cout << "debug_info: " << _debug_info << endl;
cout << "delay_mode: " << _delay_mode << endl;
cout << "vehicle_Lf: " << _Lf << endl;
cout << "frequency: " << _dt << endl;
cout << "mpc_steps: " << _mpc_steps << endl;
cout << "mpc_ref_vel: " << _ref_vel << endl;
cout << "mpc_w_cte: " << _w_cte << endl;
cout << "mpc_w_epsi: " << _w_epsi << endl;
cout << "mpc_max_steering: " << _max_steering << endl;
//Publishers and Subscribers
_sub_odom = _nh.subscribe("/odom", 1, &MPCNode::odomCB, this);
_sub_path = _nh.subscribe( _globalPath_topic, 1, &MPCNode::pathCB, this);
_sub_goal = _nh.subscribe( _goal_topic, 1, &MPCNode::goalCB, this);
_sub_amcl = _nh.subscribe("/amcl_pose", 5, &MPCNode::amclCB, this);
_pub_odompath = _nh.advertise<nav_msgs::Path>("/mpc_reference", 1); // reference path for MPC
_pub_mpctraj = _nh.advertise<nav_msgs::Path>("/mpc_trajectory", 1);// MPC trajectory output
_pub_ackermann = _nh.advertise<ackermann_msgs::AckermannDriveStamped>("/ackermann_cmd", 1);
if(_pub_twist_flag)
_pub_twist = _nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1); //for stage (Ackermann msg non-supported)
//Timer
_timer1 = _nh.createTimer(ros::Duration((1.0)/_controller_freq), &MPCNode::controlLoopCB, this); // 10Hz
//Init variables
_goal_received = false;
_goal_reached = false;
_path_computed = false;
_throttle = 0.0;
_steering = 0.0;
_speed = 0.0;
_ackermann_msg = ackermann_msgs::AckermannDriveStamped();
_twist_msg = geometry_msgs::Twist();
_mpc_traj = nav_msgs::Path();
//Init parameters for MPC object
_mpc_params["DT"] = _dt;
_mpc_params["LF"] = _Lf;
_mpc_params["STEPS"] = _mpc_steps;
_mpc_params["REF_CTE"] = _ref_cte;
_mpc_params["REF_EPSI"] = _ref_epsi;
_mpc_params["REF_V"] = _ref_vel;
_mpc_params["W_CTE"] = _w_cte;
_mpc_params["W_EPSI"] = _w_epsi;
_mpc_params["W_V"] = _w_vel;
_mpc_params["W_DELTA"] = _w_delta;
_mpc_params["W_A"] = _w_accel;
_mpc_params["W_DDELTA"] = _w_delta_d;
_mpc_params["W_DA"] = _w_accel_d;
_mpc_params["MAXSTR"] = _max_steering;
_mpc_params["MAXTHR"] = _max_throttle;
_mpc_params["BOUND"] = _bound_value;
_mpc.LoadParams(_mpc_params);
}
// Public: return _thread_numbers
int MPCNode::get_thread_numbers()
{
return _thread_numbers;
}
// Evaluate a polynomial.
double MPCNode::polyeval(Eigen::VectorXd coeffs, double x)
{
double result = 0.0;
for (int i = 0; i < coeffs.size(); i++)
{
result += coeffs[i] * pow(x, i);
}
return result;
}
// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd MPCNode::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order)
{
assert(xvals.size() == yvals.size());
assert(order >= 1 && order <= xvals.size() - 1);
Eigen::MatrixXd A(xvals.size(), order + 1);
for (int i = 0; i < xvals.size(); i++)
A(i, 0) = 1.0;
for (int j = 0; j < xvals.size(); j++)
{
for (int i = 0; i < order; i++)
A(j, i + 1) = A(j, i) * xvals(j);
}
auto Q = A.householderQr();
auto result = Q.solve(yvals);
return result;
}
// CallBack: Update odometry
void MPCNode::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
_odom = *odomMsg;
}
// CallBack: Update path waypoints (conversion to odom frame)
void MPCNode::pathCB(const nav_msgs::Path::ConstPtr& pathMsg)
{
if(_goal_received && !_goal_reached)
{
nav_msgs::Path odom_path = nav_msgs::Path();
try
{
//find waypoints distance
if(_waypointsDist <=0.0)
{
double dx = pathMsg->poses[1].pose.position.x - pathMsg->poses[0].pose.position.x;
double dy = pathMsg->poses[1].pose.position.y - pathMsg->poses[0].pose.position.y;
_waypointsDist = sqrt(dx*dx + dy*dy);
_downSampling = int(_pathLength/10.0/_waypointsDist);
}
double total_length = 0.0;
int sampling = _downSampling;
// Cut and downsampling the path
for(int i =0; i< pathMsg->poses.size(); i++)
{
if(total_length > _pathLength)
break;
if(sampling == _downSampling)
{
geometry_msgs::PoseStamped tempPose;
_tf_listener.transformPose(_odom_frame, ros::Time(0) , pathMsg->poses[i], _map_frame, tempPose);
odom_path.poses.push_back(tempPose);
sampling = 0;
}
total_length = total_length + _waypointsDist;
sampling = sampling + 1;
}
if(odom_path.poses.size() >= 6 )
{
_odom_path = odom_path; // Path waypoints in odom frame
_path_computed = true;
// publish odom path
odom_path.header.frame_id = _odom_frame;
odom_path.header.stamp = ros::Time::now();
_pub_odompath.publish(odom_path);
}
//DEBUG
//cout << endl << "N: " << odom_path.poses.size() << endl << "Car path[0]: " << odom_path.poses[0] << ", path[N]: " << _odom_path.poses[_odom_path.poses.size()-1] << endl;
}
catch(tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
}
}
// CallBack: Update goal status
void MPCNode::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg)
{
_goal_pos = goalMsg->pose.position;
_goal_received = true;
_goal_reached = false;
ROS_INFO("Goal Received !");
}
// Callback: Check if the car is inside the goal area or not
void MPCNode::amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg)
{
if(_goal_received)
{
double car2goal_x = _goal_pos.x - amclMsg->pose.pose.position.x;
double car2goal_y = _goal_pos.y - amclMsg->pose.pose.position.y;
double dist2goal = sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y);
if(dist2goal < _goalRadius)
{
_goal_reached = true;
_goal_received = false;
_path_computed = false;
ROS_INFO("Goal Reached !");
}
}
}
// Timer: Control Loop (closed loop nonlinear MPC)
void MPCNode::controlLoopCB(const ros::TimerEvent&)
{
if(_goal_received && !_goal_reached && _path_computed ) //received goal & goal not reached
{
nav_msgs::Odometry odom = _odom;
nav_msgs::Path odom_path = _odom_path;
// Update system states: X=[x, y, psi, v]
const double px = odom.pose.pose.position.x; //pose: odom frame
const double py = odom.pose.pose.position.y;
tf::Pose pose;
tf::poseMsgToTF(odom.pose.pose, pose);
const double psi = tf::getYaw(pose.getRotation());
const double v = odom.twist.twist.linear.x; //twist: body fixed frame
// Update system inputs: U=[steering, throttle]
const double steering = _steering; // radian
const double throttle = _throttle; // accel: >0; brake: <0
const double dt = _dt;
const double Lf = _Lf;
// Waypoints related parameters
const int N = odom_path.poses.size(); // Number of waypoints
const double cospsi = cos(psi);
const double sinpsi = sin(psi);
// Convert to the vehicle coordinate system
VectorXd x_veh(N);
VectorXd y_veh(N);
for(int i = 0; i < N; i++)
{
const double dx = odom_path.poses[i].pose.position.x - px;
const double dy = odom_path.poses[i].pose.position.y - py;
x_veh[i] = dx * cospsi + dy * sinpsi;
y_veh[i] = dy * cospsi - dx * sinpsi;
}
// Fit waypoints
auto coeffs = polyfit(x_veh, y_veh, 3);
const double cte = polyeval(coeffs, 0.0);
const double epsi = atan(coeffs[1]);
VectorXd state(6);
if(_delay_mode)
{
// Kinematic model is used to predict vehicle state at the actual
// moment of control (current time + delay dt)
const double px_act = v * dt;
const double py_act = 0;
const double psi_act = v * steering * dt / Lf;
const double v_act = v + throttle * dt;
const double cte_act = cte + v * sin(epsi) * dt;
const double epsi_act = -epsi + psi_act;
state << px_act, py_act, psi_act, v_act, cte_act, epsi_act;
}
else
{
state << 0, 0, 0, v, cte, epsi;
}
// Solve MPC Problem
vector<double> mpc_results = _mpc.Solve(state, coeffs);
// MPC result (all described in car frame)
_steering = mpc_results[0]; // radian
_throttle = mpc_results[1]; // acceleration
_speed = v + _throttle*dt; // speed
if (_speed >= _max_speed)
_speed = _max_speed;
if(_speed <= 0.0)
_speed = 0.0;
if(_debug_info)
{
cout << "\n\nDEBUG" << endl;
cout << "psi: " << psi << endl;
cout << "V: " << v << endl;
//cout << "odom_path: \n" << odom_path << endl;
//cout << "x_points: \n" << x_veh << endl;
//cout << "y_points: \n" << y_veh << endl;
cout << "coeffs: \n" << coeffs << endl;
cout << "_steering: \n" << _steering << endl;
cout << "_throttle: \n" << _throttle << endl;
cout << "_speed: \n" << _speed << endl;
}
// Display the MPC predicted trajectory
_mpc_traj = nav_msgs::Path();
_mpc_traj.header.frame_id = _car_frame; // points in car coordinate
_mpc_traj.header.stamp = ros::Time::now();
for(int i=0; i<_mpc.mpc_x.size(); i++)
{
geometry_msgs::PoseStamped tempPose;
tempPose.header = _mpc_traj.header;
tempPose.pose.position.x = _mpc.mpc_x[i];
tempPose.pose.position.y = _mpc.mpc_y[i];
tempPose.pose.orientation.w = 1.0;
_mpc_traj.poses.push_back(tempPose);
}
// publish the mpc trajectory
_pub_mpctraj.publish(_mpc_traj);
}
else
{
_steering = 0.0;
_throttle = 0.0;
_speed = 0.0;
if(_goal_reached && _goal_received)
cout << "Goal Reached !" << endl;
}
// publish cmd
_ackermann_msg.header.frame_id = _car_frame;
_ackermann_msg.header.stamp = ros::Time::now();
_ackermann_msg.drive.steering_angle = _steering;
_ackermann_msg.drive.speed = _speed;
_ackermann_msg.drive.acceleration = _throttle;
_pub_ackermann.publish(_ackermann_msg);
if(_pub_twist_flag)
{
_twist_msg.linear.x = _speed;
_twist_msg.angular.z = _steering;
_pub_twist.publish(_twist_msg);
}
}
/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{
//Initiate ROS
ros::init(argc, argv, "MPC_Node");
MPCNode mpc_node;
ROS_INFO("Waiting for global path msgs ~");
ros::AsyncSpinner spinner(mpc_node.get_thread_numbers()); // Use multi threads
spinner.start();
ros::waitForShutdown();
return 0;
}
#include <iostream>
#include "ros/ros.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <visualization_msgs/Marker.h>
/********************/
/* CLASS DEFINITION */
/********************/
class PurePursuit
{
public:
PurePursuit();
void initMarker();
bool isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose);
bool isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos);
double getYawFromPose(const geometry_msgs::Pose& carPose);
double getEta(const geometry_msgs::Pose& carPose);
double getCar2GoalDist();
double getSteering(double eta);
geometry_msgs::Point get_odom_car2WayPtVec(const geometry_msgs::Pose& carPose);
private:
ros::NodeHandle n_;
ros::Subscriber odom_sub, path_sub, goal_sub, amcl_sub;
ros::Publisher ackermann_pub, cmdvel_pub, marker_pub;
ros::Timer timer1, timer2;
tf::TransformListener tf_listener;
visualization_msgs::Marker points, line_strip, goal_circle;
geometry_msgs::Point odom_goal_pos, goal_pos;
geometry_msgs::Twist cmd_vel;
ackermann_msgs::AckermannDriveStamped ackermann_cmd;
nav_msgs::Odometry odom;
nav_msgs::Path map_path, odom_path;
double L, Lfw, Vcmd, lfw, steering, velocity;
double steering_gain, base_angle, goal_radius, speed_incremental;
int controller_freq;
bool foundForwardPt, goal_received, goal_reached, cmd_vel_mode, debug_mode, smooth_accel;
void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);
void pathCB(const nav_msgs::Path::ConstPtr& pathMsg);
void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg);
void amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg);
void controlLoopCB(const ros::TimerEvent&);
}; // end of class
PurePursuit::PurePursuit()
{
//Private parameters handler
ros::NodeHandle pn("~");
//Car parameter
pn.param("L", L, 0.26); // length of car
pn.param("Vcmd", Vcmd, 1.0);// reference speed (m/s)
pn.param("Lfw", Lfw, 3.0); // forward look ahead distance (m)
pn.param("lfw", lfw, 0.13); // distance between front the center of car
//Controller parameter
pn.param("controller_freq", controller_freq, 20);
pn.param("steering_gain", steering_gain, 1.0);
pn.param("goal_radius", goal_radius, 0.5); // goal radius (m)
pn.param("base_angle", base_angle, 0.0); // neutral point of servo (rad)
pn.param("cmd_vel_mode", cmd_vel_mode, false); // whether or not publishing cmd_vel
pn.param("debug_mode", debug_mode, false); // debug mode
pn.param("smooth_accel", smooth_accel, true); // smooth the acceleration of car
pn.param("speed_incremental", speed_incremental, 0.5); // speed incremental value (discrete acceleraton), unit: m/s
//Publishers and Subscribers
odom_sub = n_.subscribe("/pure_pursuit/odom", 1, &PurePursuit::odomCB, this);
path_sub = n_.subscribe("/pure_pursuit/global_planner", 1, &PurePursuit::pathCB, this);
goal_sub = n_.subscribe("/pure_pursuit/goal", 1, &PurePursuit::goalCB, this);
amcl_sub = n_.subscribe("/amcl_pose", 5, &PurePursuit::amclCB, this);
marker_pub = n_.advertise<visualization_msgs::Marker>("/pure_pursuit/path_marker", 10);
ackermann_pub = n_.advertise<ackermann_msgs::AckermannDriveStamped>("/pure_pursuit/ackermann_cmd", 1);
if(cmd_vel_mode) cmdvel_pub = n_.advertise<geometry_msgs::Twist>("/pure_pursuit/cmd_vel", 1);
//Timer
timer1 = n_.createTimer(ros::Duration((1.0)/controller_freq), &PurePursuit::controlLoopCB, this); // Duration(0.05) -> 20Hz
//Init variables
foundForwardPt = false;
goal_received = false;
goal_reached = false;
velocity = 0.0;
steering = base_angle;
//Show info
ROS_INFO("[param] base_angle: %f", base_angle);
ROS_INFO("[param] Vcmd: %f", Vcmd);
ROS_INFO("[param] Lfw: %f", Lfw);
//Visualization Marker Settings
initMarker();
cmd_vel = geometry_msgs::Twist();
ackermann_cmd = ackermann_msgs::AckermannDriveStamped();
}
void PurePursuit::initMarker()
{
points.header.frame_id = line_strip.header.frame_id = goal_circle.header.frame_id = "odom";
points.ns = line_strip.ns = goal_circle.ns = "Markers";
points.action = line_strip.action = goal_circle.action = visualization_msgs::Marker::ADD;
points.pose.orientation.w = line_strip.pose.orientation.w = goal_circle.pose.orientation.w = 1.0;
points.id = 0;
line_strip.id = 1;
goal_circle.id = 2;
points.type = visualization_msgs::Marker::POINTS;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
goal_circle.type = visualization_msgs::Marker::CYLINDER;
// POINTS markers use x and y scale for width/height respectively
points.scale.x = 0.2;
points.scale.y = 0.2;
//LINE_STRIP markers use only the x component of scale, for the line width
line_strip.scale.x = 0.1;
goal_circle.scale.x = goal_radius;
goal_circle.scale.y = goal_radius;
goal_circle.scale.z = 0.1;
// Points are green
points.color.g = 1.0f;
points.color.a = 1.0;
// Line strip is blue
line_strip.color.b = 1.0;
line_strip.color.a = 1.0;
//goal_circle is yellow
goal_circle.color.r = 1.0;
goal_circle.color.g = 1.0;
goal_circle.color.b = 0.0;
goal_circle.color.a = 0.5;
}
void PurePursuit::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
this->odom = *odomMsg;
}
void PurePursuit::pathCB(const nav_msgs::Path::ConstPtr& pathMsg)
{
this->map_path = *pathMsg;
}
void PurePursuit::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goalMsg)
{
this->goal_pos = goalMsg->pose.position;
try
{
geometry_msgs::PoseStamped odom_goal;
tf_listener.transformPose("odom", ros::Time(0) , *goalMsg, "map" ,odom_goal);
odom_goal_pos = odom_goal.pose.position;
goal_received = true;
goal_reached = false;
/*Draw Goal on RVIZ*/
goal_circle.pose = odom_goal.pose;
marker_pub.publish(goal_circle);
}
catch(tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
}
double PurePursuit::getYawFromPose(const geometry_msgs::Pose& carPose)
{
float x = carPose.orientation.x;
float y = carPose.orientation.y;
float z = carPose.orientation.z;
float w = carPose.orientation.w;
double tmp,yaw;
tf::Quaternion q(x,y,z,w);
tf::Matrix3x3 quaternion(q);
quaternion.getRPY(tmp,tmp, yaw);
return yaw;
}
bool PurePursuit::isForwardWayPt(const geometry_msgs::Point& wayPt, const geometry_msgs::Pose& carPose)
{
float car2wayPt_x = wayPt.x - carPose.position.x;
float car2wayPt_y = wayPt.y - carPose.position.y;
double car_theta = getYawFromPose(carPose);
float car_car2wayPt_x = cos(car_theta)*car2wayPt_x + sin(car_theta)*car2wayPt_y;
float car_car2wayPt_y = -sin(car_theta)*car2wayPt_x + cos(car_theta)*car2wayPt_y;
if(car_car2wayPt_x >0) /*is Forward WayPt*/
return true;
else
return false;
}
bool PurePursuit::isWayPtAwayFromLfwDist(const geometry_msgs::Point& wayPt, const geometry_msgs::Point& car_pos)
{
double dx = wayPt.x - car_pos.x;
double dy = wayPt.y - car_pos.y;
double dist = sqrt(dx*dx + dy*dy);
if(dist < Lfw)
return false;
else if(dist >= Lfw)
return true;
}
geometry_msgs::Point PurePursuit::get_odom_car2WayPtVec(const geometry_msgs::Pose& carPose)
{
geometry_msgs::Point carPose_pos = carPose.position;
double carPose_yaw = getYawFromPose(carPose);
geometry_msgs::Point forwardPt;
geometry_msgs::Point odom_car2WayPtVec;
foundForwardPt = false;
if(!goal_reached){
for(int i =0; i< map_path.poses.size(); i++)
{
geometry_msgs::PoseStamped map_path_pose = map_path.poses[i];
geometry_msgs::PoseStamped odom_path_pose;
try
{
tf_listener.transformPose("odom", ros::Time(0) , map_path_pose, "map" ,odom_path_pose);
geometry_msgs::Point odom_path_wayPt = odom_path_pose.pose.position;
bool _isForwardWayPt = isForwardWayPt(odom_path_wayPt,carPose);
if(_isForwardWayPt)
{
bool _isWayPtAwayFromLfwDist = isWayPtAwayFromLfwDist(odom_path_wayPt,carPose_pos);
if(_isWayPtAwayFromLfwDist)
{
forwardPt = odom_path_wayPt;
foundForwardPt = true;
break;
}
}
}
catch(tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
}
}
else if(goal_reached)
{
forwardPt = odom_goal_pos;
foundForwardPt = false;
//ROS_INFO("goal REACHED!");
}
/*Visualized Target Point on RVIZ*/
/*Clear former target point Marker*/
points.points.clear();
line_strip.points.clear();
if(foundForwardPt && !goal_reached)
{
points.points.push_back(carPose_pos);
points.points.push_back(forwardPt);
line_strip.points.push_back(carPose_pos);
line_strip.points.push_back(forwardPt);
}
marker_pub.publish(points);
marker_pub.publish(line_strip);
odom_car2WayPtVec.x = cos(carPose_yaw)*(forwardPt.x - carPose_pos.x) + sin(carPose_yaw)*(forwardPt.y - carPose_pos.y);
odom_car2WayPtVec.y = -sin(carPose_yaw)*(forwardPt.x - carPose_pos.x) + cos(carPose_yaw)*(forwardPt.y - carPose_pos.y);
return odom_car2WayPtVec;
}
double PurePursuit::getEta(const geometry_msgs::Pose& carPose)
{
geometry_msgs::Point odom_car2WayPtVec = get_odom_car2WayPtVec(carPose);
return atan2(odom_car2WayPtVec.y,odom_car2WayPtVec.x);
}
double PurePursuit::getCar2GoalDist()
{
geometry_msgs::Point car_pose = this->odom.pose.pose.position;
double car2goal_x = this->odom_goal_pos.x - car_pose.x;
double car2goal_y = this->odom_goal_pos.y - car_pose.y;
return sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y);
}
double PurePursuit::getSteering(double eta)
{
return atan2((this->L*sin(eta)),(this->Lfw/2 + this->lfw*cos(eta)));
}
void PurePursuit::amclCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amclMsg)
{
if(this->goal_received)
{
double car2goal_x = this->goal_pos.x - amclMsg->pose.pose.position.x;
double car2goal_y = this->goal_pos.y - amclMsg->pose.pose.position.y;
double dist2goal = sqrt(car2goal_x*car2goal_x + car2goal_y*car2goal_y);
if(dist2goal < this->goal_radius)
{
this->goal_reached = true;
this->goal_received = false;
ROS_INFO("Goal Reached !");
}
}
}
void PurePursuit::controlLoopCB(const ros::TimerEvent&)
{
geometry_msgs::Pose carPose = this->odom.pose.pose;
geometry_msgs::Twist carVel = this->odom.twist.twist;
if(this->goal_received)
{
/*Estimate Steering Angle*/
double eta = getEta(carPose);
if(foundForwardPt)
{
this->steering = this->base_angle + getSteering(eta)*this->steering_gain;
/*Estimate Gas Input*/
if(!this->goal_reached)
{
if(this->smooth_accel)
this->velocity = std::min(this->velocity + this->speed_incremental, this->Vcmd);
else
this->velocity = this->Vcmd;
if(debug_mode) ROS_INFO("Velocity = %.2f, Steering = %.2f", this->velocity, this->steering);
}
}
}
if(this->goal_reached)
{
this->velocity = 0.0;
this->steering = this->base_angle;
}
this->ackermann_cmd.drive.steering_angle = this->steering;
this->ackermann_cmd.drive.speed = this->velocity;
this->ackermann_pub.publish(this->ackermann_cmd);
if(this->cmd_vel_mode)
{
this->cmd_vel.linear.x = this->velocity;
this->cmd_vel.angular.z = this->steering;
this->cmdvel_pub.publish(this->cmd_vel);
}
}
/*****************/
/* MAIN FUNCTION */
/*****************/
int main(int argc, char **argv)
{
//Initiate ROS
ros::init(argc, argv, "PurePursuit");
PurePursuit controller;
ros::AsyncSpinner spinner(2); // Use multi threads
spinner.start();
ros::waitForShutdown();
return 0;
}
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明阿克曼运动模型（ackermann）的一些资料-室外光电组参考资料！
