阿克曼运动模型（ackermann）的一些资料 |
您所在的位置：网站首页 › 阿克曼模型转弯转弯时速度变吗 › 阿克曼运动模型（ackermann）的一些资料 |
阿克曼运动模型（ackermann）的一些资料
|
最常见的移动机器人模型，差速，麦克纳姆轮的全向，阿克曼车式等。
直道行驶--视觉
弯道行驶--视觉 全国大学生智能汽车竞赛-室外光电组无人驾驶挑战赛-2019 https://blog.csdn.net/ZhangRelay/article/details/89639965 仿真环境（激光雷达lidar）：
实车测试（激光雷达lidar）：
左转和右转半径示意 主流的仿真软件都提供了这些模型的机器人，如webots,V-rep,Gazebo.
结构示意如上图。 重点参考： http://wiki.ros.org/Ackermann%20Grouphttp://wiki.ros.org/stepback_and_steerturn_recovery部分代码只支持遥控，并不支持此类（ackermann）模型的导航，原生的ROS导航包也不能直接使用，需要调整。 ROS官网wiki有ackermann兴趣小组：为Ackermann机器人的控制，导航和模拟仿真生成通用接口。 teb_local_plannerstepback_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 #include #include // 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("cmd_vel", 10); recover_run_pub_ = nh_.advertise("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 " 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 dist_to_obstacle_r; vector 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 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 #include #include #include "ros/ros.h" #include #include #include #include #include #include #include #include #include #include "MPC.h" #include #include 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 _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("global_path_topic", _globalPath_topic, "/move_base/TrajectoryPlannerROS/global_plan" ); pn.param("goal_topic", _goal_topic, "/move_base_simple/goal" ); pn.param("map_frame", _map_frame, "map" ); pn.param("odom_frame", _odom_frame, "odom"); pn.param("car_frame", _car_frame, "base_link" ); //Display the parameters cout |
【本文地址】
今日新闻 |
推荐新闻 |