61制作工厂网站,秦皇岛网络科技有限公司,江津区建设工程交易中心网站,程序开发平台哪个好适用场景
1. 机器人要尽可能走直线
2. 遇到障碍物需要机器人停住, 不去尝试绕开障碍物效果
机器人在收到目标点后, global_planner先生成一条直达该点的路径机器人转向目标点机器人移动至目标点机器人旋转到目标位姿
全局路径规划
具体可以参考上篇文章, 修改了ROS自带navi…适用场景
1. 机器人要尽可能走直线
2. 遇到障碍物需要机器人停住, 不去尝试绕开障碍物效果
机器人在收到目标点后, global_planner先生成一条直达该点的路径机器人转向目标点机器人移动至目标点机器人旋转到目标位姿
全局路径规划
具体可以参考上篇文章, 修改了ROS自带navigation包中的carrot_planner, 使之具有以下特点
global_plan这个vector中包含的路径点的数量增加, 适配局部路径规划为global_plan中各个路径点添加位姿可以将global_plan生成的路径以posearray的形式发布出来, 可以在rviz中监测 图中的红色密集箭头即为global_plan生成的全局路径, 话题为 /move_base_node/SimplePlanner/global_path
局部路径规划
参照ftc_local_planner进行修改, 特点如下 在重复修改终点时, 机器人不会有明显的减速, 停止, 再加速的过程 添加在旋转前的障碍物判定, 模拟自身旋转一周, 如果有碰撞风险则阻止此次旋转(参考…忘了抄的那里的了, 应该是dwa吧?) base_local_planner::WorldModel* world_model_; /// brief The world model that the controller will use double FTCPlannerROS::footprintCost(double x_i, double y_i, double theta_i){std::vectorgeometry_msgs::Point footprint costmap-getRobotFootprint();double footprint_cost world_model_-footprintCost(x_i, y_i, theta_i, footprint);return footprint_cost;}//模拟旋转一周, 判断自身是否有几率碰撞到周围的障碍物bool FTCPlannerROS::isRotateLegal(){// ROS_ERROR(isRotateLegal is calling!!!);const double full_rotation_range 2 * M_PI; // 360 degreesconst double angle_increment 0.01;// 这里的currenet_control_point是ftc在运行过程中生成的控制点, 本质是在global_plan中提取出来的一个位姿double currentX current_control_point.translation().x();double currentY current_control_point.translation().y();double currentYaw current_control_point.rotation().eulerAngles(0, 1, 2).z();for (double angle 0; angle full_rotation_range; angle angle_increment) {double rotate_footprint_cost footprintCost(currentX, currentY,(currentYaw angle));if(rotate_footprint_cost costmap_2d::LETHAL_OBSTACLE){return false;} }return true;}完整代码
ftc_planner.hpp
#ifndef FTC_LOCAL_PLANNER_FTC_PLANNER_H_
#define FTC_LOCAL_PLANNER_FTC_PLANNER_H_#include ros/ros.h
#include ftc_local_planner/PlannerGetProgress.h
#include ftc_local_planner/recovery_behaviors.h#include nav_msgs/Odometry.h
#include nav_msgs/Path.h
#include std_msgs/Bool.h
#include costmap_2d/costmap_2d_ros.h
#include tf/transform_listener.h
#include dynamic_reconfigure/server.h
#include ftc_local_planner/FTCPlannerROSConfig.h
#include ftc_local_planner/PID.h
#include nav_core/base_local_planner.h
#include tf2_ros/transform_broadcaster.h
#include tf2/LinearMath/Quaternion.h
#include tf2_geometry_msgs/tf2_geometry_msgs.h
#include nav_msgs/Odometry.h
#include tf2_ros/transform_listener.h
#include Eigen/Geometry
#include tf2_eigen/tf2_eigen.h
#include visualization_msgs/Marker.h
#include base_local_planner/world_model.h
#include base_local_planner/costmap_model.h
#include base_local_planner/odometry_helper_ros.h
namespace ftc_local_planner
{class FTCPlannerROS : public nav_core::BaseLocalPlanner{enum PlannerState{PRE_ROTATE,FOLLOWING,WAITING_FOR_GOAL_APPROACH,POST_ROTATE,FINISHED};private:ros::ServiceServer progress_server;// State trackingPlannerState current_state;ros::Time state_entered_time;bool is_crashed;dynamic_reconfigure::ServerFTCPlannerROSConfig *reconfig_server;tf2_ros::Buffer *tf_buffer;costmap_2d::Costmap2DROS *costmap;costmap_2d::Costmap2D* costmap_map_; base_local_planner::WorldModel* world_model_; /// brief The world model that the controller will usestd::vectorgeometry_msgs::PoseStamped global_plan;ros::Publisher global_point_pub;ros::Publisher global_plan_pub;ros::Publisher progress_pub;ros::Publisher obstacle_marker_pub;ros::Publisher obstacle_observer_pub;FTCPlannerROSConfig config;Eigen::Affine3d current_control_point;/*** PID State*/double lat_error, lon_error, angle_error 0.0;double last_lon_error 0.0;double last_lat_error 0.0;double last_angle_error 0.0;double i_lon_error 0.0;double i_lat_error 0.0;double i_angle_error 0.0;ros::Time last_time;/*** Speed ramp for acceleration and deceleration*/double current_movement_speed;/*** State for point interpolation*/uint32_t current_index;uint32_t obstacle_index;double current_progress;Eigen::Affine3d local_control_point;/*** Private members*/ros::Publisher pubPid;FailureDetector failure_detector_; //! Detect if the robot got stuckedros::Time time_last_oscillation_; //! Store at which time stamp the last oscillation was detectedbool oscillation_detected_ false;bool oscillation_warning_ false;/*** odom_helper */ base_local_planner::OdometryHelperRos odom_helper_;std::string odom_topic_;double distanceLookahead();PlannerState update_planner_state();void update_control_point(double dt);void calculate_velocity_commands(double dt, geometry_msgs::Twist cmd_vel);/*** brief check for obstacles in path as well as collision at actual pose* param max_points number of path segments (of global path) to check* return true if collision will happen.*/bool checkCollision(int max_points);double footprintCost(double x_i, double y_i, double theta_i);bool isRotateLegal();/*** brief check if robot oscillates (only angular). Can be used to do some recovery* param cmd_vel last velocity message send to robot* return true if robot oscillates*/bool checkOscillation(geometry_msgs::Twist cmd_vel);/*** brief publish obstacles on path as marker array.* brief If obstacle_points contains more elements than maxID, marker gets published and* brief cleared afterwards.* param obstacle_points already collected points to visualize* param x X position in costmap* param y Y position in costmap* param cost cost value of cell* param maxIDs num of markers before publishing* return sum of values, or 0.0 if values is empty.*/void debugObstacle(visualization_msgs::Marker obstacle_points, double x, double y, unsigned char cost, int maxIDs);double time_in_current_state(){return (ros::Time::now() - state_entered_time).toSec();}void reconfigureCB(FTCPlannerROSConfig config, uint32_t level);public:FTCPlannerROS();~FTCPlannerROS();bool getProgress(ftc_local_planner::PlannerGetProgressRequest req, ftc_local_planner::PlannerGetProgressResponse res);bool setPlan(const std::vectorgeometry_msgs::PoseStamped plan);void initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros);bool computeVelocityCommands(geometry_msgs::Twist cmd_vel);bool isGoalReached();bool cancel();};
};
#endifftc_planner.cpp
#include ftc_local_planner/ftc_planner.h
#include pluginlib/class_list_macros.hPLUGINLIB_EXPORT_CLASS(ftc_local_planner::FTCPlannerROS, nav_core::BaseLocalPlanner)#define RET_SUCCESS 0
#define RET_COLLISION 104
#define RET_BLOCKED 109namespace ftc_local_planner
{FTCPlannerROS::FTCPlannerROS():odom_helper_(odom){}void FTCPlannerROS::initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros){ros::NodeHandle private_nh(~/ name);progress_server private_nh.advertiseService(planner_get_progress, FTCPlannerROS::getProgress, this);global_point_pub private_nh.advertisegeometry_msgs::PoseStamped(global_point, 1);global_plan_pub private_nh.advertisenav_msgs::Path(global_plan, 1, true);obstacle_marker_pub private_nh.advertisevisualization_msgs::Marker(costmap_marker, 10);obstacle_observer_pub private_nh.advertisestd_msgs::Bool(obstacle_detection, 10);costmap costmap_ros;costmap_map_ costmap-getCostmap();world_model_ new base_local_planner::CostmapModel(*costmap_map_); tf_buffer tf;// Parameter for dynamic reconfigurereconfig_server new dynamic_reconfigure::ServerFTCPlannerROSConfig(private_nh);dynamic_reconfigure::ServerFTCPlannerROSConfig::CallbackType cb boost::bind(FTCPlannerROS::reconfigureCB, this,_1, _2);reconfig_server-setCallback(cb);current_state PRE_ROTATE;// PID Debugging topicif (config.debug_pid){pubPid private_nh.advertiseftc_local_planner::PID(debug_pid, 1, true);}// Recovery behavior initializationfailure_detector_.setBufferLength(std::round(config.oscillation_recovery_min_duration * 10));ROS_INFO(FTCLocalPlannerROS: Version 2 Init.);}void FTCPlannerROS::reconfigureCB(FTCPlannerROSConfig c, uint32_t level){if (c.restore_defaults){reconfig_server-getConfigDefault(c);c.restore_defaults false;}config c;// just to be surecurrent_movement_speed config.speed_slow;// set recovery behaviorfailure_detector_.setBufferLength(std::round(config.oscillation_recovery_min_duration * 10));}bool FTCPlannerROS::setPlan(const std::vectorgeometry_msgs::PoseStamped plan){current_state PRE_ROTATE;state_entered_time ros::Time::now();is_crashed false;global_plan plan;// if whole_distance is less than 0.1m then follow in detail, // else set control_point further to avoid discrete speed control.double whole_distance, diffX, diffY;diffX global_plan[0].pose.position.x - global_plan[global_plan.size()-1].pose.position.x;diffY global_plan[0].pose.position.y - global_plan[global_plan.size()-1].pose.position.y;whole_distance std::sqrt(diffX * diffX diffY * diffY);// if length of whole path is over 1 [m], set current_index aheadif(whole_distance 1) current_index 18;else current_index 1;obstacle_index 0;current_progress 0.0;last_time ros::Time::now();current_movement_speed config.speed_slow;lat_error 0.0;lon_error 0.0;angle_error 0.0;i_lon_error 0.0;i_lat_error 0.0;i_angle_error 0.0;nav_msgs::Path path;if (global_plan.size() 2){// duplicate last pointglobal_plan.push_back(global_plan.back());// give second from last point last oriantation as the point before thatglobal_plan[global_plan.size() - 2].pose.orientation global_plan[global_plan.size() - 3].pose.orientation;path.header plan.front().header;path.poses plan;}else{ROS_WARN_STREAM(FTCLocalPlannerROS: Global plan was too short. Need a minimum of 3 poses - Cancelling.);current_state FINISHED;state_entered_time ros::Time::now();}global_plan_pub.publish(path);ROS_INFO_STREAM(FTCLocalPlannerROS: Got new global plan with plan.size() points.);return true;}FTCPlannerROS::~FTCPlannerROS(){if (reconfig_server ! nullptr){delete reconfig_server;reconfig_server nullptr;}delete world_model_;}double FTCPlannerROS::distanceLookahead(){if (global_plan.size() 2){return 0;}Eigen::Quaterniondouble current_rot(current_control_point.linear());Eigen::Affine3d last_straight_point current_control_point;for (uint32_t i current_index 1; i global_plan.size(); i){tf2::fromMsg(global_plan[i].pose, last_straight_point);// check, if direction is the same. if so, we add the distanceEigen::Quaterniondouble rot2(last_straight_point.linear());if (abs(rot2.angularDistance(current_rot)) config.speed_fast_threshold_angle * (M_PI / 180.0)){break;}}return (last_straight_point.translation() - current_control_point.translation()).norm();}bool FTCPlannerROS::computeVelocityCommands(geometry_msgs::Twist cmd_vel){ros::Time now ros::Time::now();double dt now.toSec() - last_time.toSec();last_time now;if (is_crashed){cmd_vel.linear.x 0;cmd_vel.angular.z 0;return false;}if (current_state FINISHED){cmd_vel.linear.x 0;cmd_vel.angular.z 0;return true;}// Were not crashed and not finished.// First, we update the control point if needed. This is needed since we need the local_control_point to calculate the next state.update_control_point(dt);// Then, update the planner state.auto new_planner_state update_planner_state();if (new_planner_state ! current_state){ROS_INFO_STREAM(FTCLocalPlannerROS: Switching to state new_planner_state);state_entered_time ros::Time::now();current_state new_planner_state;}if (checkCollision(config.obstacle_lookahead)){cmd_vel.linear.x 0;cmd_vel.angular.z 0;is_crashed true;return false;}// Finally, we calculate the velocity commands.calculate_velocity_commands(dt, cmd_vel);if (is_crashed){cmd_vel.linear.x 0;cmd_vel.angular.z 0;return false;}return true;}bool FTCPlannerROS::isGoalReached(){return current_state FINISHED !is_crashed;}bool FTCPlannerROS::cancel(){ROS_WARN_STREAM(FTCLocalPlannerROS: FTC planner was cancelled.);current_state FINISHED;state_entered_time ros::Time::now();return true;}FTCPlannerROS::PlannerState FTCPlannerROS::update_planner_state(){switch (current_state){case PRE_ROTATE:{if (time_in_current_state() config.goal_timeout){ROS_ERROR_STREAM(FTCLocalPlannerROS: Error reaching goal. config.goal_timeout ( config.goal_timeout ) reached - Timeout in PRE_ROTATE phase.);is_crashed true;return FINISHED;}// calculate total distance, STOP if whole path is too shortdouble whole_distance, diffX, diffY;diffX global_plan[0].pose.position.x - global_plan[global_plan.size()-1].pose.position.x;diffY global_plan[0].pose.position.y - global_plan[global_plan.size()-1].pose.position.y;whole_distance std::sqrt(diffX * diffX diffY * diffY);if (whole_distance 0.08) {ROS_ERROR(FTC plan is too short, abort!);return FINISHED;}if (abs(angle_error) * (180.0 / M_PI) config.max_goal_angle_error || !isRotateLegal()){std::coutcurrent angle error: angle_errorstd::endl;std::coutcurrent angle error in degree: abs(angle_error) * (180.0 / M_PI)std::endl;std::coutconfig.max_goal_angle_error: config.max_goal_angle_errorstd::endl;ROS_INFO_STREAM(FTCLocalPlannerROS: PRE_ROTATE finished. Starting following);ROS_INFO(Switching state to FOLLOWING!!!);return FOLLOWING;}}break;case FOLLOWING:{double distance local_control_point.translation().norm();// check for crashif (distance config.max_follow_distance){ROS_ERROR_STREAM(FTCLocalPlannerROS: Robot is far away from global plan. distance ( distance ) config.max_follow_distance ( config.max_follow_distance ) It probably has crashed.);is_crashed true;return FINISHED;}// check if were done followingif (current_index global_plan.size() - 2){ROS_INFO_STREAM(FTCLocalPlannerROS: switching planner to position mode);ROS_INFO(FOLLOWING finished!);ROS_INFO(Switching to WAITING_FOR_GOAL_APPROACH);return WAITING_FOR_GOAL_APPROACH;}}break;case WAITING_FOR_GOAL_APPROACH:{double distance local_control_point.translation().norm();if (time_in_current_state() config.goal_timeout){ROS_WARN_STREAM(FTCLocalPlannerROS: Could not reach goal position. config.goal_timeout ( config.goal_timeout ) reached - Attempting final rotation anyways.);return POST_ROTATE;}if (distance config.max_goal_distance_error){ROS_INFO(WAITING_FOR_GOAL_APPROACH finished!);ROS_INFO(Switching to POST_ROTATE!);ROS_INFO_STREAM(FTCLocalPlannerROS: Reached goal position.);return POST_ROTATE;}}break;case POST_ROTATE:{if (time_in_current_state() config.goal_timeout){ROS_WARN_STREAM(FTCLocalPlannerROS: Could not reach goal rotation. config.goal_timeout ( config.goal_timeout ) reached);return FINISHED;}// calculate total distancedouble total_distance0.0;double diffX;double diffY;if (current_control_point.translation().x()){diffX current_control_point.translation().x() - global_plan[global_plan.size()-1].pose.position.x;diffY current_control_point.translation().y() - global_plan[global_plan.size()-1].pose.position.y;}else{diffX global_plan[0].pose.position.x - global_plan[global_plan.size()-1].pose.position.x;diffY global_plan[0].pose.position.y - global_plan[global_plan.size()-1].pose.position.y;}total_distance std::sqrt(diffX * diffX diffY * diffY);if (total_distance 0.08) {ROS_ERROR(FTC plan is too short, abort!);return FINISHED;}if (abs(angle_error) * (180.0 / M_PI) config.max_goal_angle_error || !isRotateLegal()){ROS_INFO_STREAM(FTCLocalPlannerROS: POST_ROTATE finished.);return FINISHED;}}break;case FINISHED:{ROS_INFO(Move finished!);}break;}return current_state;}void FTCPlannerROS::update_control_point(double dt){switch (current_state){case PRE_ROTATE:{nav_msgs::Odometry current_odom;odom_helper_.getOdom(current_odom);// if current speed is over 0.02 [m/s] set control_point ahead.if(abs(current_odom.twist.twist.linear.x)0.02) tf2::fromMsg(global_plan[current_index].pose, current_control_point);elsetf2::fromMsg(global_plan[1].pose, current_control_point);}break;case FOLLOWING:{// Normal planner operationdouble straight_dist distanceLookahead();double speed;if (straight_dist config.speed_fast_threshold){speed config.speed_fast;}else{speed config.speed_slow;}if (speed current_movement_speed){// acceleratecurrent_movement_speed dt * config.acceleration;if (current_movement_speed speed)current_movement_speed speed;}else if (speed current_movement_speed){// deceleratecurrent_movement_speed - dt * config.acceleration;if (current_movement_speed speed)current_movement_speed speed;}double distance_to_move dt * current_movement_speed;double angle_to_move dt * config.speed_angular * (M_PI / 180.0);Eigen::Affine3d nextPose, currentPose;while (angle_to_move 0 distance_to_move 0 current_index global_plan.size() - 2){tf2::fromMsg(global_plan[current_index].pose, currentPose);tf2::fromMsg(global_plan[current_index 1].pose, nextPose);double pose_distance (nextPose.translation() - currentPose.translation()).norm();Eigen::Quaterniondouble current_rot(currentPose.linear());Eigen::Quaterniondouble next_rot(nextPose.linear());double pose_distance_angular current_rot.angularDistance(next_rot);if (pose_distance 0.0){ROS_WARN_STREAM(FTCLocalPlannerROS: Skipping duplicate point in global plan.);current_index;obstacle_index;continue;}double remaining_distance_to_next_pose pose_distance * (1.0 - current_progress);double remaining_angular_distance_to_next_pose pose_distance_angular * (1.0 - current_progress);if (remaining_distance_to_next_pose distance_to_move remaining_angular_distance_to_next_pose angle_to_move){// we need to move further than the remaining distance_to_move. Skip to the next point and decrease distance_to_move.current_progress 0.0;current_index;obstacle_index;distance_to_move - remaining_distance_to_next_pose;angle_to_move - remaining_angular_distance_to_next_pose;}else{// we cannot reach the next point yet, so we update the percentagedouble current_progress_distance (pose_distance * current_progress distance_to_move) / pose_distance;double current_progress_angle (pose_distance_angular * current_progress angle_to_move) / pose_distance_angular;current_progress fmin(current_progress_angle, current_progress_distance);if (current_progress 1.0){ROS_WARN_STREAM(FTCLocalPlannerROS: FTC PLANNER: Progress 1.0);// current_progress 1.0;}distance_to_move 0;angle_to_move 0;}}tf2::fromMsg(global_plan[current_index].pose, currentPose);tf2::fromMsg(global_plan[current_index 1].pose, nextPose);// interpolate between pointsEigen::Quaterniondouble rot1(currentPose.linear());Eigen::Quaterniondouble rot2(nextPose.linear());Eigen::Vector3d trans1 currentPose.translation();Eigen::Vector3d trans2 nextPose.translation();Eigen::Affine3d result;result.translation() (1.0 - current_progress) * trans1 current_progress * trans2;result.linear() rot1.slerp(current_progress, rot2).toRotationMatrix();current_control_point result;}break;case POST_ROTATE:tf2::fromMsg(global_plan[global_plan.size() - 1].pose, current_control_point);break;case WAITING_FOR_GOAL_APPROACH:break;case FINISHED:break;}{geometry_msgs::PoseStamped viz;viz.header global_plan[current_index].header;viz.pose tf2::toMsg(current_control_point);global_point_pub.publish(viz);}auto map_to_base tf_buffer-lookupTransform(base_link, map, ros::Time(), ros::Duration(1.0));tf2::doTransform(current_control_point, local_control_point, map_to_base);lat_error local_control_point.translation().y();lon_error local_control_point.translation().x();angle_error local_control_point.rotation().eulerAngles(0, 1, 2).z();}void FTCPlannerROS::calculate_velocity_commands(double dt, geometry_msgs::Twist cmd_vel){// check, if were completely doneif (current_state FINISHED || is_crashed){ROS_DEBUG_STREAM(current stateFINISHED!, set cme_vel 0.0);cmd_vel.linear.x 0;cmd_vel.angular.z 0;return;}i_lon_error lon_error * dt;i_lat_error lat_error * dt;i_angle_error angle_error * dt;if (i_lon_error config.ki_lon_max){i_lon_error config.ki_lon_max;}else if (i_lon_error -config.ki_lon_max){i_lon_error -config.ki_lon_max;}if (i_lat_error config.ki_lat_max){i_lat_error config.ki_lat_max;}else if (i_lat_error -config.ki_lat_max){i_lat_error -config.ki_lat_max;}if (i_angle_error config.ki_ang_max){i_angle_error config.ki_ang_max;}else if (i_angle_error -config.ki_ang_max){i_angle_error -config.ki_ang_max;}double d_lat (lat_error - last_lat_error) / dt;double d_lon (lon_error - last_lon_error) / dt;double d_angle (angle_error - last_angle_error) / dt;last_lat_error lat_error;last_lon_error lon_error;last_angle_error angle_error;// linear movement is only allowed in FOLLOWING state// calculate linear speedif (current_state FOLLOWING){double lin_speed lon_error * config.kp_lon i_lon_error * config.ki_lon d_lon * config.kd_lon;if (lin_speed 0 config.forward_only){lin_speed 0;}else{if (lin_speed config.max_cmd_vel_speed){lin_speed config.max_cmd_vel_speed;}else if (lin_speed -config.max_cmd_vel_speed){lin_speed -config.max_cmd_vel_speed;}if (lin_speed 0){lat_error * -1.0;}}cmd_vel.linear.x lin_speed;}else{cmd_vel.linear.x 0.0;}// calculate angular speedif (current_state FOLLOWING){double ang_speed angle_error * config.kp_ang i_angle_error * config.ki_ang d_angle * config.kd_ang lat_error * config.kp_lat i_lat_error * config.ki_lat d_lat * config.kd_lat;if (ang_speed config.max_cmd_vel_ang){ang_speed config.max_cmd_vel_ang;}else if (ang_speed -config.max_cmd_vel_ang){ang_speed -config.max_cmd_vel_ang;}// check whether obstacle exists while rotatingif (!isRotateLegal()){ang_speed 0;is_crashed true;ROS_ERROR_STREAM(STOP ROTATE!!! Obstacle near by!);}cmd_vel.angular.z ang_speed;}else{double ang_speed angle_error * config.kp_ang i_angle_error * config.ki_ang d_angle * config.kd_ang;if (ang_speed config.max_cmd_vel_ang){ang_speed config.max_cmd_vel_ang;}else if (ang_speed -config.max_cmd_vel_ang){ang_speed -config.max_cmd_vel_ang;}// check whether obstacle exists while rotatingif (!isRotateLegal()){ang_speed 0;is_crashed true;ROS_ERROR_STREAM(STOP ROTATE!!! Obstacle near by!);}cmd_vel.angular.z ang_speed;// check if robot oscillatesbool is_oscillating checkOscillation(cmd_vel);if (is_oscillating){ang_speed config.max_cmd_vel_ang;cmd_vel.angular.z ang_speed;}}if (config.debug_pid){ftc_local_planner::PID debugPidMsg;debugPidMsg.kp_lon_set lon_error;// proportionaldebugPidMsg.kp_lat_set lat_error * config.kp_lat;debugPidMsg.kp_lon_set lon_error * config.kp_lon;debugPidMsg.kp_ang_set angle_error * config.kp_ang;// integraldebugPidMsg.ki_lat_set i_lat_error * config.ki_lat;debugPidMsg.ki_lon_set i_lon_error * config.ki_lon;debugPidMsg.ki_ang_set i_angle_error * config.ki_ang;// diffdebugPidMsg.kd_lat_set d_lat * config.kd_lat;debugPidMsg.kd_lon_set d_lon * config.kd_lon;debugPidMsg.kd_ang_set d_angle * config.kd_ang;// errorsdebugPidMsg.lon_err lon_error;debugPidMsg.lat_err lat_error;debugPidMsg.ang_err angle_error;// speedsdebugPidMsg.ang_speed cmd_vel.angular.z;debugPidMsg.lin_speed cmd_vel.linear.x;pubPid.publish(debugPidMsg);}}bool FTCPlannerROS::getProgress(PlannerGetProgressRequest req, PlannerGetProgressResponse res){res.index current_index;return true;}bool FTCPlannerROS::checkCollision(int max_points){unsigned int x;unsigned int y;std::vectorgeometry_msgs::Point footprint;visualization_msgs::Marker obstacle_marker;if (!config.check_obstacles){return false;}// maximal costsunsigned char previous_cost 255;// ensure look ahead not out of planif (global_plan.size() max_points){max_points global_plan.size();}// calculate cost of footprint at robots actual poseif (config.obstacle_footprint){costmap-getOrientedFootprint(footprint);for (int i 0; i footprint.size(); i){// check cost of each point of footprintif (costmap_map_-worldToMap(footprint[i].x, footprint[i].y, x, y)){unsigned char costs costmap_map_-getCost(x, y);if (costs costmap_2d::LETHAL_OBSTACLE){ROS_WARN(FTCLocalPlannerROS: Possible collision of footprint at actual pose. Stop local planner.);std_msgs::Bool obstacle_msg;obstacle_msg.data true;obstacle_observer_pub.publish(obstacle_msg);return true;}}}}for (int i 0; i max_points; i){geometry_msgs::PoseStamped x_pose;int index obstacle_index i;if (index global_plan.size()){index global_plan.size();}x_pose global_plan[index];if (costmap_map_-worldToMap(x_pose.pose.position.x, x_pose.pose.position.y, x, y)){unsigned char costs costmap_map_-getCost(x, y);if (config.debug_obstacle){debugObstacle(obstacle_marker, x, y, costs, max_points);}// Near at obstacelif (costs 0){// Possible collisionif (costs 127 costs previous_cost){ROS_WARN(FTCLocalPlannerROS: Possible collision. Stop local planner.);std_msgs::Bool obstacle_msg;obstacle_msg.data true;obstacle_observer_pub.publish(obstacle_msg); return true;}}previous_cost costs;}}return false;}bool FTCPlannerROS::isRotateLegal(){// ROS_ERROR(isRotateLegal is calling!!!);const double full_rotation_range 2 * M_PI; // 360 degreesconst double angle_increment 0.01;double currentX current_control_point.translation().x();double currentY current_control_point.translation().y();double currentYaw current_control_point.rotation().eulerAngles(0, 1, 2).z();for (double angle 0; angle full_rotation_range; angle angle_increment) {double rotate_footprint_cost footprintCost(currentX, currentY,(currentYaw angle));if(rotate_footprint_cost costmap_2d::LETHAL_OBSTACLE){ROS_DEBUG_STREAM();ROS_DEBUG_STREAM(FTC: Rotate is illegal);ROS_DEBUG_STREAM(FTC: switch to next state!);ROS_DEBUG_STREAM();return false;} }return true;}double FTCPlannerROS::footprintCost(double x_i, double y_i, double theta_i){std::vectorgeometry_msgs::Point footprint costmap-getRobotFootprint();double footprint_cost world_model_-footprintCost(x_i, y_i, theta_i, footprint);return footprint_cost;}bool FTCPlannerROS::checkOscillation(geometry_msgs::Twist cmd_vel){bool oscillating false;// detect and resolve oscillationsif (config.oscillation_recovery){// oscillating true;double max_vel_theta config.max_cmd_vel_ang;double max_vel_current config.max_cmd_vel_speed;failure_detector_.update(cmd_vel, config.max_cmd_vel_speed, config.max_cmd_vel_speed, max_vel_theta,config.oscillation_v_eps, config.oscillation_omega_eps);oscillating failure_detector_.isOscillating();if (oscillating) // we are currently oscillating{if (!oscillation_detected_) // do we already know that robot oscillates?{time_last_oscillation_ ros::Time::now(); // save time when oscillation was detectedoscillation_detected_ true;}// calculate duration of actual oscillationbool oscillation_duration_timeout !((ros::Time::now() - time_last_oscillation_).toSec() config.oscillation_recovery_min_duration); // check how long we oscillateif (oscillation_duration_timeout){if (!oscillation_warning_) // ensure to send warning just once instead of spamming around{ROS_WARN(FTCLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).);oscillation_warning_ true;}return true;}return false; // oscillating but timeout not reached}else{// not oscillatingtime_last_oscillation_ ros::Time::now(); // save time when oscillation was detectedoscillation_detected_ false;oscillation_warning_ false;return false;}}return false; // no check for oscillation}void FTCPlannerROS::debugObstacle(visualization_msgs::Marker obstacle_points, double x, double y, unsigned char cost, int maxIDs){if (obstacle_points.points.empty()){obstacle_points.header.frame_id costmap-getGlobalFrameID();obstacle_points.header.stamp ros::Time::now();obstacle_points.action visualization_msgs::Marker::ADD;obstacle_points.pose.orientation.w 1.0;obstacle_points.type visualization_msgs::Marker::POINTS;obstacle_points.scale.x 0.2;obstacle_points.scale.y 0.2;}obstacle_points.id obstacle_points.points.size() 1;if (cost 127){obstacle_points.color.g 1.0f;}if (cost 127 cost 255){obstacle_points.color.r 1.0f;}obstacle_points.color.a 1.0;geometry_msgs::Point p;costmap_map_-mapToWorld(x, y, p.x, p.y);p.z 0;obstacle_points.points.push_back(p);if (obstacle_points.points.size() maxIDs || cost 0){obstacle_marker_pub.publish(obstacle_points);obstacle_points.points.clear();}}
}
