阿克曼运动模型（ackermann）的一些资料-室外光电组参考资料

最常见的移动机器人模型，差速，麦克纳姆轮的全向，阿克曼车式等。

直道行驶--视觉

弯道行驶--视觉

全国大学生智能汽车竞赛-室外光电组无人驾驶挑战赛-2019

https://blog.csdn.net/ZhangRelay/article/details/89639965

仿真环境（激光雷达lidar）：

实车测试（激光雷达lidar）：

左转和右转半径示意

主流的仿真软件都提供了这些模型的机器人，如webots,V-rep,Gazebo.

结构示意如上图。

重点参考：

http://wiki.ros.org/Ackermann%20Group
http://wiki.ros.org/stepback_and_steerturn_recovery

部分代码只支持遥控，并不支持此类（ackermann）模型的导航，原生的ROS导航包也不能直接使用，需要调整。

ROS官网wiki有ackermann兴趣小组：为Ackermann机器人的控制，导航和模拟仿真生成通用接口。

teb_local_planner
stepback_and_steerturn_recovery

设置并测试优化

在本教程中，学习如何运行轨迹优化以及如何更改基础参数以设置自定义行为和性能。
检查优化反馈
在本教程中，学习如何检查优化轨迹的反馈; 提供了一个示例，其可视化当前所选轨迹的速度分布。
配置并运行机器人导航
在本教程中，学习如何将teb_local_planner设置为导航的本地规划程序插件。
避障和机器人轨迹模型
在本教程中，了解如何实现避障。描述了主要关注机器人轨迹模型及其影响的必要参数设置。
遵循全局规划（Via-Points）
在本教程中，学习如何配置局部规划程序以更严格地遵循全局规划。特别是，学习如何调整时间最优性和路径跟踪之间的权衡。
代价地图转换
在本教程中，学习如何应用costmap转换插件将已占用的costmap2d单元格转换为几何图元以进行优化（实验）。
规划类似汽车（ackermann）的机器人
在本教程中，学习如何为类似汽车的机器人设置计划器（实验）。
规划完整机器人
在本教程中，学习如何设置完整机器人的计划器（实验）。
考虑定制的障碍
在本教程中，学习如何将从其他节点发布的多边形障碍考虑在内。
考虑动态障碍
在本教程中，学习如何将从其他节点发布的动态障碍考虑在内。
通过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);// backprivate_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);//-- steerprivate_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);//-- forwardprivate_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 invalidgm::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 outif(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 obstacleif(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 costconst 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 rightelseret_val = LEFT; // vice versareturn 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 truerun_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 behaviorconst gm::Pose2D& initialPose = getCurrentLocalPose();// initial poseROS_DEBUG_NAMED ("top", "initial pose (%.2f, %.2f, %.2f)", initialPose.x,initialPose.y, initialPose.theta);ros::Rate r(controller_frequency_);// step backbase_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);// stopfor (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 steeringgm::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 straightROS_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 steeringROS_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");}// stopfor (double t=0; t<stop_duaration; t+=1/controller_frequency_) {cmd_vel_pub_.publish(TWIST_STOP);r.sleep();}// check trial timesif(cnt == trial_times_){ROS_INFO_NAMED ("top", "break after %d times recovery", cnt);break;}// check clearance forwardconst  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 falserun_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

其他相关资料参考：

https://blog.csdn.net/ZhangRelay/article/details/89639965

推荐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 classL1Controller::L1Controller()
{//Private parameters handlerros::NodeHandle pn("~");//Car parameterpn.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 parameterpn.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 Subscribersodom_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);//Timertimer1 = n_.createTimer(ros::Duration((1.0)/controller_freq), &L1Controller::controlLoopCB, this); // Duration(0.05) -> 20Hztimer2 = n_.createTimer(ros::Duration((0.5)/controller_freq), &L1Controller::goalReachingCB, this); // Duration(0.05) -> 20Hz//Init variablesLfw = goalRadius = getL1Distance(Vcmd);foundForwardPt = false;goal_received = false;goal_reached = false;cmd_vel.linear.x = 1500; // 1500 for stopcmd_vel.angular.z = baseAngle;//Show infoROS_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 SettingsinitMarker();
}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 respectivelypoints.scale.x = 0.2;points.scale.y = 0.2;//LINE_STRIP markers use only the x component of scale, for the line widthline_strip.scale.x = 0.1;goal_circle.scale.x = goalRadius;goal_circle.scale.y = goalRadius;goal_circle.scale.z = 0.1;// Points are greenpoints.color.g = 1.0f;points.color.a = 1.0;// Line strip is blueline_strip.color.b = 1.0;line_strip.color.a = 1.0;//goal_circle is yellowgoal_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;elsereturn 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;elseL1 = 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 ROSros::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 classMPCNode::MPCNode()
{//Private parameters handlerros::NodeHandle pn("~");//Parameters for control looppn.param("thread_numbers", _thread_numbers, 2); // number of threads for this ROS nodepn.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/spn.param("waypoints_dist", _waypointsDist, -1.0); // unit: mpn.param("path_length", _pathLength, 8.0); // unit: mpn.param("goal_radius", _goalRadius, 0.5); // unit: mpn.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 solverpn.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 accelpn.param("mpc_bound_value", _bound_value, 1.0e3); // Bound value for other variables//Parameter for topics & Frame namepn.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 parameterscout << "\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 distanceif(_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 pathfor(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 pathodom_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 frameconst 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;  // radianconst double throttle = _throttle; // accel: >0; brake: <0const double dt = _dt;const double Lf = _Lf;// Waypoints related parametersconst int N = odom_path.poses.size(); // Number of waypointsconst double cospsi = cos(psi);const double sinpsi = sin(psi);// Convert to the vehicle coordinate systemVectorXd 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 waypointsauto 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 Problemvector<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;  // speedif (_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 ROSros::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 threadsspinner.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 classPurePursuit::PurePursuit()
{//Private parameters handlerros::NodeHandle pn("~");//Car parameterpn.param("L", L, 0.26); // length of carpn.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 parameterpn.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_velpn.param("debug_mode", debug_mode, false); // debug modepn.param("smooth_accel", smooth_accel, true); // smooth the acceleration of carpn.param("speed_incremental", speed_incremental, 0.5); // speed incremental value (discrete acceleraton), unit: m/s//Publishers and Subscribersodom_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);    //Timertimer1 = n_.createTimer(ros::Duration((1.0)/controller_freq), &PurePursuit::controlLoopCB, this); // Duration(0.05) -> 20Hz//Init variablesfoundForwardPt = false;goal_received = false;goal_reached = false;velocity = 0.0;steering = base_angle;//Show infoROS_INFO("[param] base_angle: %f", base_angle);ROS_INFO("[param] Vcmd: %f", Vcmd);ROS_INFO("[param] Lfw: %f", Lfw);//Visualization Marker SettingsinitMarker();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 respectivelypoints.scale.x = 0.2;points.scale.y = 0.2;//LINE_STRIP markers use only the x component of scale, for the line widthline_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 greenpoints.color.g = 1.0f;points.color.a = 1.0;// Line strip is blueline_strip.color.b = 1.0;line_strip.color.a = 1.0;//goal_circle is yellowgoal_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;elsereturn 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);elsethis->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 ROSros::init(argc, argv, "PurePursuit");PurePursuit controller;ros::AsyncSpinner spinner(2); // Use multi threadsspinner.start();ros::waitForShutdown();return 0;
}

阿克曼运动模型（ackermann）的一些资料-室外光电组参考资料相关推荐

第十五届全国大学生智能汽车竞赛室外光电组全国总决赛方案
第十五届全国大学生智能汽车竞赛室外光电组全国总决赛方案 1. 赛事背景全国大学生智能汽车竞赛是以智能汽车为研究对象的创意性科技竞赛,是面向全国大学生的一种具有探索性工程的实践活动,是教育部倡导的大 ...
第十五届全国大学生智能车竞赛室外光电组线上选拔赛比赛流程
第十五届全国大学生智能车竞赛室外光电组线上选拔赛比赛流程作者:卓晴博士,清华大学自动化系更新时间:2020-07-30 Thursday 1.线上选拔赛将采取腾讯会议直播与电脑录屏结合的方式进行 ...
2019年大学生智能车大赛室外光电组+在ROS下搭建仿真模拟环境，编程控制小车完成定位导航仿真
2019年大学生智能车大赛室外光电组+在ROS下搭建仿真模拟环境,编程控制小车完成定位导航仿真一.前言二.准备工作 1.创建工作空间 2.下载racecar源代码包,并编译工程三.启动仿真 1. ...
2019年大学生智能车大赛室外光电组+在ROS下搭建仿真模拟环境，编程控制小车，按照给定赛道完成比赛
大学生智能车大赛室外光电组,在ROS下搭建仿真模拟环境一.前言二.效果图三.准备工作 [1.在Ubuntu上安装ROS Kinetic](https://blog.csdn.net/qq_425 ...
全国大学生智能车室外光电组补充资料（偏方版本）
关于比赛正式资料参考钢铁侠和智能车竞赛官方网站.之前,博客涉及"偏方"如下: 来龙去脉:https://blog.csdn.net/ZhangRelay/article/detai ...
ROS学习:智能车室外光电组仿真
一.模型建立首先先准备我们的开源仿真包mit-racecar,博主已经把资源上传到CSDN上了.需要请自取. https://download.csdn.net/download/weixin_44 ...
智能车仿真 —— 2020室外光电组仿真指导（一）
前言今天推出了官方的仿真文件,我也参与了部分仿真模型的设计,提供一个简单的参考思路. 完整文章地址:古月居博客网站模型下载地址 https://pan.baidu.com/s/1TFBTbCeyQ ...
第十五届全国大学生智能汽车竞赛-室外光电ROS组预赛方案
01赛事背景全国大学生智能汽车竞赛是以智能汽车为研究对象的创意性科技竞赛,是面向全国大学生的一种具有探索性工程的实践活动,是教育部倡导的大学生科技竞赛之一.竞赛以立足培养,重在参与,鼓励探索,追求卓 ...
ROS | Gazebo仿真—阿克曼（Ackermann）四轮小车模型
ROS | Gazebo仿真-阿克曼(Ackermann)四轮小车模型 1. 仿真描述 2. Gazebo简介 2.1 Gazebo的典型用途 2.2 Gazebo的主要特点 3. ros_contr ...
北邮智能车仿真培训（九）—— 室外光电创意组仿真
前言 2020.5.1跟新了racecar,阿克曼模型更适合比赛,大家可以直接看这里:室外光电创意组racecar 前面的教程基本上是常规组别的,主要用于给大家写下摄像头的算法然后体会下PID,今天的 ...

阿克曼运动模型（ackermann）的一些资料-室外光电组参考资料

阿克曼运动模型（ackermann）的一些资料-室外光电组参考资料相关推荐

最新文章

热门文章