teb双向行驶

如下图在teb_local_planner_ros.h文件添加

bool last_inverse=false;

 修改teb_local_planner_ros.cpp中的computeVelocityCommands函数。

大致实现思路是当机器人姿态和全局路径成钝角时，对当前点姿态取反来规划，规划出的速度也取反。

bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
  // check if plugin initialized
  if(!initialized_)
  {
    ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner");
    return false;
  }

  cmd_vel.linear.x = 0;
  cmd_vel.linear.y = 0;
  cmd_vel.angular.z = 0;
  goal_reached_ = false;  
  
  // Get robot pose
  tf::Stamped<tf::Pose> robot_pose;
  costmap_ros_->getRobotPose(robot_pose);
  robot_pose_ = PoseSE2(robot_pose);

  //std::cout<<"robot_pose_: "<<robot_pose_<<std::endl;
    
  // Get robot velocity
  tf::Stamped<tf::Pose> robot_vel_tf;
  odom_helper_.getRobotVel(robot_vel_tf);
  robot_vel_.linear.x = robot_vel_tf.getOrigin().getX();
  robot_vel_.linear.y = robot_vel_tf.getOrigin().getY();
  robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation());

  
  
  // prune global plan to cut off parts of the past (spatially before the robot)
  pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);

  // Transform global plan to the frame of interest (w.r.t. the local costmap)
  std::vector<geometry_msgs::PoseStamped> transformed_plan;
  int goal_idx;
  tf::StampedTransform tf_plan_to_global;
  if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, 
                           transformed_plan, &goal_idx, &tf_plan_to_global))
  {
    ROS_WARN("Could not transform the global plan to the frame of the controller");
    return false;
  }

  // update via-points container
  if (!custom_via_points_active_)
    updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);

  // check if global goal is reached
  tf::Stamped<tf::Pose> global_goal;
  tf::poseStampedMsgToTF(global_plan_.back(), global_goal);
  global_goal.setData( tf_plan_to_global * global_goal );
  double dx = global_goal.getOrigin().getX() - robot_pose_.x();
  double dy = global_goal.getOrigin().getY() - robot_pose_.y();
  double delta_orient = g2o::normalize_theta( tf::getYaw(global_goal.getRotation()) - robot_pose_.theta() );
  if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance
    && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance
    && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0))
  {
    goal_reached_ = true;
    return true;
  }
  
  
  // check if we should enter any backup mode and apply settings
  configureBackupModes(transformed_plan, goal_idx);
  
    
  // Return false if the transformed global plan is empty
  if (transformed_plan.empty())
  {
    ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");
    return false;
  }
              
  // Get current goal point (last point of the transformed plan)
  tf::Stamped<tf::Pose> goal_point;
  tf::poseStampedMsgToTF(transformed_plan.back(), goal_point);
  robot_goal_.x() = goal_point.getOrigin().getX();
  robot_goal_.y() = goal_point.getOrigin().getY();      
  if (cfg_.trajectory.global_plan_overwrite_orientation)
  {
    robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point, goal_idx, tf_plan_to_global);
    // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)
    transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta());
  }  
  else
  {
    robot_goal_.theta() = tf::getYaw(goal_point.getRotation());
  }

  double x_offset = transformed_plan.back().pose.position.x -transformed_plan.front().pose.position.x;
  double y_offset = transformed_plan.back().pose.position.y -transformed_plan.front().pose.position.y;
  double theta = std::atan2(y_offset,x_offset);
  transformed_plan.back().pose.orientation=tf::createQuaternionMsgFromYaw(theta);
  
  //zhc add 
  bool Isinverse=false;
  tf::Stamped<tf::Pose> transformed_start_goal;
  tf::poseStampedMsgToTF(transformed_plan.front(), transformed_start_goal);
  double transform_start_yaw = tf::getYaw(transformed_start_goal.getRotation());

  if(std::fabs(transform_start_yaw-robot_pose_.theta())>=1.6)
  {
    Isinverse=true;
    //std::cout<<"start robot_pose_.theta(): "<<robot_pose_.theta()<<std::endl;
    if(robot_pose_.theta()<0)
    {

      robot_pose_.theta()+=3.1415926;
    }
    else
    {
      robot_pose_.theta()-=3.1415926;
    }

    //tf::Quaternion orientation_;
    //tf::quaternionMsgToTF(transformed_plan.back().pose.orientation, orientation_);
    //transformed_plan.back().pose.orientation=tf::createQuaternionMsgFromYaw(-tf::getYaw(orientation_));
    robot_vel_.linear.x = -robot_vel_tf.getOrigin().getX();
    robot_vel_.linear.y = -robot_vel_tf.getOrigin().getY();
  }

  // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory)
  if (transformed_plan.size()==1) // plan only contains the goal
  {
    transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized)
  }

  tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // update start;

  if(!Isinverse)
  {
    tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // update start;
  }
  else
  {
    std::cout<<"end robot_pose_.theta(): "<<robot_pose_.theta()<<std::endl;
    transformed_plan.front().pose.position.x=robot_pose_.x();
    transformed_plan.front().pose.position.y=robot_pose_.y();
    transformed_plan.front().pose.orientation=tf::createQuaternionMsgFromYaw(robot_pose_.theta());
  }

    
  // clear currently existing obstacles
  obstacles_.clear();
  
  // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin
  if (costmap_converter_)
    updateObstacleContainerWithCostmapConverter();
  else
    updateObstacleContainerWithCostmap();
  
  // also consider custom obstacles (must be called after other updates, since the container is not cleared)
  updateObstacleContainerWithCustomObstacles();
  
    
  // Do not allow config changes during the following optimization step
  boost::mutex::scoped_lock cfg_lock(cfg_.configMutex());

  // Now perform the actual planning
//   bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_.goal_tolerance.free_goal_vel); // straight line init
  bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
  if (!success)
  {
    planner_->clearPlanner(); // force reinitialization for next time
    ROS_WARN("teb_local_planner was not able to obtain a local plan for the current setting.");
    
    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel;
    return false;
  }

         
  // Check feasibility (but within the first few states only)
  if(cfg_.robot.is_footprint_dynamic)
  {
    // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
    footprint_spec_ = costmap_ros_->getRobotFootprint();
    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);
  }

  bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);
  if (!feasible)
  {
    cmd_vel.linear.x = 0;
    cmd_vel.linear.y = 0;
    cmd_vel.angular.z = 0;
   
    // now we reset everything to start again with the initialization of new trajectories.
    planner_->clearPlanner();
    ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner...");
    
    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel;
    return false;
  }
 
  //std::cout<<"66cmd_vel.linear.x: "<<cmd_vel.linear.x<<std::endl;

  // Get the velocity command for this sampling interval
  if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, cfg_.trajectory.control_look_ahead_poses))
  {
    planner_->clearPlanner();
    ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner...");
    ++no_infeasible_plans_; // increase number of infeasible solutions in a row
    time_last_infeasible_plan_ = ros::Time::now();
    last_cmd_ = cmd_vel;
    return false;
  }
  
  // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).
  saturateVelocity(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, cfg_.robot.max_vel_x, cfg_.robot.max_vel_y,
                   cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);

  if(Isinverse)
  {
    cmd_vel.linear.x=-cmd_vel.linear.x;
    cmd_vel.linear.y=-cmd_vel.linear.y;
  }

  // convert rot-vel to steering angle if desired (carlike robot).
  // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
  // and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself.
  if (cfg_.robot.cmd_angle_instead_rotvel)
  {
    cmd_vel.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.linear.x, cmd_vel.angular.z, cfg_.robot.wheelbase, 0.95*cfg_.robot.min_turning_radius);
    if (!std::isfinite(cmd_vel.angular.z))
    {
      cmd_vel.linear.x = cmd_vel.linear.y = cmd_vel.angular.z = 0;
      last_cmd_ = cmd_vel;
      planner_->clearPlanner();
      ROS_WARN("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner...");
      ++no_infeasible_plans_; // increase number of infeasible solutions in a row
      time_last_infeasible_plan_ = ros::Time::now();
      return false;
    }
  }
  
  // a feasible solution should be found, reset counter
  no_infeasible_plans_ = 0;
  
  // store last command (for recovery analysis etc.)
  if(Isinverse)
  {
    last_inverse=true;
    last_cmd_.linear.x=-cmd_vel.linear.x;
    last_cmd_.linear.y=-cmd_vel.linear.y;
  }
  else
  {
    last_cmd_ = cmd_vel;
  }

  // Now visualize everything    
  planner_->visualize();
  visualization_->publishObstacles(obstacles_);
  visualization_->publishViaPoints(via_points_);
  visualization_->publishGlobalPlan(global_plan_);
  return true;
}