cl_nav2z::forward_local_planner::ForwardLocalPlanner Class Reference

#include <forward_local_planner.hpp>

Inheritance diagram for cl_nav2z::forward_local_planner::ForwardLocalPlanner:

Collaboration diagram for cl_nav2z::forward_local_planner::ForwardLocalPlanner:

Public Member Functions
	ForwardLocalPlanner ()
virtual	~ForwardLocalPlanner ()
void	configure (const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, const std::shared_ptr< tf2_ros::Buffer > tf, const std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros) override
void	activate () override
void	deactivate () override
void	cleanup () override
void	setPlan (const nav_msgs::msg::Path &path) override
	nav2_core setPlan - Sets the global plan
virtual geometry_msgs::msg::TwistStamped	computeVelocityCommands (const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::Twist &velocity, nav2_core::GoalChecker *goal_checker) override
	nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity
bool	isGoalReached ()
virtual void	setSpeedLimit (const double &speed_limit, const bool &percentage) override

Private Member Functions
void	updateParameters ()
void	publishGoalMarker (double x, double y, double phi)
void	cleanMarkers ()
void	generateTrajectory (const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, float maxdist, float maxangle, float maxtime, float dt, std::vector< Eigen::Vector3f > &outtraj)
Eigen::Vector3f	computeNewPositions (const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)

Private Attributes
nav2_util::LifecycleNode::SharedPtr	nh_
std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmapRos_
std::string	name_
rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray >::SharedPtr	goalMarkerPublisher_
double	k_rho_
double	k_alpha_
double	k_betta_
bool	goalReached_
const double	alpha_offset_ = 0
const double	betta_offset_ = 0
meter	carrot_distance_
rad	carrot_angular_distance_
double	yaw_goal_tolerance_
double	xy_goal_tolerance_
double	max_angular_z_speed_
double	max_linear_x_speed_
double	transform_tolerance_
int	currentPoseIndex_
std::vector< geometry_msgs::msg::PoseStamped >	plan_
bool	waiting_
rclcpp::Duration	waitingTimeout_
rclcpp::Time	waitingStamp_
std::shared_ptr< tf2_ros::Buffer >	tf_

Detailed Description

Definition at line 38 of file forward_local_planner.hpp.

Constructor & Destructor Documentation

ForwardLocalPlanner()

cl_nav2z::forward_local_planner::ForwardLocalPlanner::ForwardLocalPlanner

(

)

ForwardLocalPlanner()

Definition at line 42 of file forward_local_planner.cpp.

: transform_tolerance_(0.1), waitingTimeout_(2s) {}

~ForwardLocalPlanner()

cl_nav2z::forward_local_planner::ForwardLocalPlanner::~ForwardLocalPlanner ( )

virtual

Definition at line 44 of file forward_local_planner.cpp.

{}

Member Function Documentation

activate()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::activate ( )

override

Definition at line 46 of file forward_local_planner.cpp.

{
  RCLCPP_DEBUG_STREAM(nh_->get_logger(), "activating controller ForwardLocalPlanner");
  this->updateParameters();
  this->goalMarkerPublisher_->on_activate();
}

References goalMarkerPublisher_, nh_, and updateParameters().

Here is the call graph for this function:

cleanMarkers()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::cleanMarkers ( )

private

Definition at line 256 of file forward_local_planner.cpp.

{
  RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardLocalPlanner] cleaning markers.");
  visualization_msgs::msg::Marker marker;
 
  marker.header.frame_id = costmapRos_->getGlobalFrameID();
  marker.header.stamp = nh_->now();
  marker.ns = "my_namespace2";
  marker.id = 0;
  marker.action = visualization_msgs::msg::Marker::DELETEALL;
 
  visualization_msgs::msg::MarkerArray ma;
  ma.markers.push_back(marker);
 
  goalMarkerPublisher_->publish(ma);
}

References costmapRos_, goalMarkerPublisher_, and nh_.

Referenced by cleanup(), and deactivate().

Here is the caller graph for this function:

cleanup()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::cleanup ( )

override

Definition at line 59 of file forward_local_planner.cpp.

{
  this->cleanMarkers();
  this->plan_.clear();
  this->currentPoseIndex_ = 0;
  yaw_goal_tolerance_ = -1;
  xy_goal_tolerance_ = -1;
}

References cleanMarkers(), currentPoseIndex_, plan_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Here is the call graph for this function:

computeNewPositions()

Eigen::Vector3f cl_nav2z::forward_local_planner::ForwardLocalPlanner::computeNewPositions ( const Eigen::Vector3f & pos, const Eigen::Vector3f & vel, double dt )

private

Definition at line 200 of file forward_local_planner.cpp.

{
  Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
  new_pos[0] = pos[0] + (static_cast<double>(vel[0]) * cos(pos[2]) +
                         static_cast<double>(vel[1]) * cos(M_PI_2 + pos[2])) *
                          dt;
  new_pos[1] = pos[1] + (static_cast<double>(vel[0]) * sin(pos[2]) +
                         static_cast<double>(vel[1]) * sin(M_PI_2 + pos[2])) *
                          dt;
  new_pos[2] = pos[2] + vel[2] * dt;
  return new_pos;
}

Referenced by generateTrajectory().

Here is the caller graph for this function:

computeVelocityCommands()

geometry_msgs::msg::TwistStamped cl_nav2z::forward_local_planner::ForwardLocalPlanner::computeVelocityCommands ( const geometry_msgs::msg::PoseStamped & currentPose, const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker )

overridevirtual

nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity

It is presumed that the global plan is already set.

This is mostly a wrapper for the protected computeVelocityCommands function which has additional debugging info.

Parameters

pose	Current robot pose
velocity	Current robot velocity

Returns

The best command for the robot to drive

computeVelocityCommands()

Definition at line 313 of file forward_local_planner.cpp.

{
  this->updateParameters();
 
  if (this->plan_.size() > 0)
  {
    RCLCPP_INFO_STREAM(
      nh_->get_logger(), "[ForwardLocalPlanner] Current pose frame id: "
                           << plan_.front().header.frame_id
                           << ", path pose frame id: " << currentPose.header.frame_id);
 
    if (plan_.front().header.frame_id != currentPose.header.frame_id)
    {
      RCLCPP_ERROR_STREAM(nh_->get_logger(), "[ForwardLocalPlanner] Inconsistent frames");
    }
  }
 
  // xy_goal_tolerance and yaw_goal_tolerance are just used for logging proposes and clamping the carrot
  // goal distance (parameter safety)
  if (xy_goal_tolerance_ == -1 || yaw_goal_tolerance_ == -1)
  {
    geometry_msgs::msg::Pose posetol;
    geometry_msgs::msg::Twist twistol;
    if (goal_checker->getTolerances(posetol, twistol))
    {
      xy_goal_tolerance_ = posetol.position.x;
      yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation);
      //xy_goal_tolerance_ = posetol.position.x * 0.35;  // WORKAROUND DIFFERENCE WITH NAV CONTROLLER GOAL CHECKER
      //yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation) * 0.35;
      RCLCPP_INFO_STREAM(
        nh_->get_logger(), "[ForwardLocalPlanner] xy_goal_tolerance_: " << xy_goal_tolerance_
                                                                        << ", yaw_goal_tolerance_: "
                                                                        << yaw_goal_tolerance_);
    }
    else
    {
      RCLCPP_INFO_STREAM(
        nh_->get_logger(), "[ForwardLocalPlanner] could not get tolerances from goal checker");
    }
  }
 
  geometry_msgs::msg::TwistStamped cmd_vel;
  goalReached_ = false;
  RCLCPP_DEBUG(
    nh_->get_logger(), "[ForwardLocalPlanner] ----- COMPUTE VELOCITY COMMAND LOCAL PLANNER ---");
 
  bool ok = false;
  while (!ok)
  {
    // iterate the point from the current position and ahead until reaching a new goal point in the path
    while (!ok && currentPoseIndex_ < (int)plan_.size())
    {
      auto & pose = plan_[currentPoseIndex_];
      const geometry_msgs::msg::Point & p = pose.pose.position;
      tf2::Quaternion q;
      tf2::fromMsg(pose.pose.orientation, q);
 
      // take error from the current position to the path point
      double dx = p.x - currentPose.pose.position.x;
      double dy = p.y - currentPose.pose.position.y;
      double dist = sqrt(dx * dx + dy * dy);
 
      double pangle = tf2::getYaw(q);
      double angle = tf2::getYaw(currentPose.pose.orientation);
      double angular_error = angles::shortest_angular_distance(pangle, angle);
 
      if (dist >= carrot_distance_ || fabs(angular_error) > 0.1)
      {
        // the target pose is enough different to be defined as a target
        ok = true;
        RCLCPP_DEBUG(
          nh_->get_logger(),
          "current index: %d, carrot goal percentaje: %lf, dist: %lf, maxdist: %lf, angle_error: "
          "%lf",
          currentPoseIndex_, 100.0 * currentPoseIndex_ / plan_.size(), dist, carrot_distance_,
          angular_error);
      }
      else
      {
        currentPoseIndex_++;
      }
    }
 
    RCLCPP_DEBUG_STREAM(
      nh_->get_logger(), "[ForwardLocalPlanner] selected carrot pose index "
                           << currentPoseIndex_ << "/" << plan_.size());
 
    if (currentPoseIndex_ >= (int)plan_.size())
    {
      // even the latest point is quite similar, then take the last since it is the final goal
      cmd_vel.twist.linear.x = 0;
      cmd_vel.twist.angular.z = 0;
      // RCLCPP_INFO(nh_->get_logger(), "End Local planner");
      ok = true;
      currentPoseIndex_ = (int)plan_.size() - 1;
    }
  }
 
  // RCLCPP_INFO(nh_->get_logger(), "pose control algorithm");
 
  const geometry_msgs::msg::PoseStamped & finalgoalpose = plan_.back();
  const geometry_msgs::msg::PoseStamped & carrot_goalpose = plan_[currentPoseIndex_];
  const geometry_msgs::msg::Point & goalposition = carrot_goalpose.pose.position;
 
  tf2::Quaternion carrotGoalQ;
  tf2::fromMsg(carrot_goalpose.pose.orientation, carrotGoalQ);
  // RCLCPP_INFO_STREAM(nh_->get_logger(), "Plan goal quaternion at "<< carrot_goalpose.pose.orientation);
 
  // goal orientation (global frame)
  double betta = tf2::getYaw(carrot_goalpose.pose.orientation) + betta_offset_;
  double dx = goalposition.x - currentPose.pose.position.x;
  double dy = goalposition.y - currentPose.pose.position.y;
 
  // distance error to the targetpoint
  double rho_error = sqrt(dx * dx + dy * dy);
 
  tf2::Quaternion currentOrientation;
  tf2::convert(currentPose.pose.orientation, currentOrientation);
 
  // current angle
  double theta = tf2::getYaw(currentOrientation);
  double alpha = atan2(dy, dx);
  alpha = alpha + alpha_offset_;
 
  double alpha_error = angles::shortest_angular_distance(alpha, theta);
  double betta_error = angles::shortest_angular_distance(betta, theta);
 
  double vetta = 0;  // = k_rho_ * rho_error;
  double gamma = 0;  //= k_alpha_ * alpha_error + k_betta_ * betta_error;
 
  if (
    rho_error >
    xy_goal_tolerance_)  // regular control rule, be careful, rho error is with the carrot not with the
                         // final goal (this is something to improve like the backwards planner)
  {
    vetta = k_rho_ * rho_error;
    gamma = k_alpha_ * alpha_error;
  }
  else if (fabs(betta_error) >= yaw_goal_tolerance_)  // pureSpining
  {
    vetta = 0;
    gamma = k_betta_ * betta_error;
  }
  else  // goal reached
  {
    RCLCPP_DEBUG(nh_->get_logger(), "GOAL REACHED");
    vetta = 0;
    gamma = 0;
    goalReached_ = true;
  }
 
  // linear speed clamp
  if (vetta > max_linear_x_speed_)
  {
    vetta = max_linear_x_speed_;
  }
  else if (vetta < -max_linear_x_speed_)
  {
    vetta = -max_linear_x_speed_;
  }
 
  // angular speed clamp
  if (gamma > max_angular_z_speed_)
  {
    gamma = max_angular_z_speed_;
  }
  else if (gamma < -max_angular_z_speed_)
  {
    gamma = -max_angular_z_speed_;
  }
 
  cmd_vel.twist.linear.x = vetta;
  cmd_vel.twist.angular.z = gamma;
 
  publishGoalMarker(goalposition.x, goalposition.y, betta);
 
  RCLCPP_DEBUG_STREAM(
    nh_->get_logger(), "Forward local planner,"
                         << std::endl
                         << " theta: " << theta << std::endl
                         << " betta: " << betta << std::endl
                         << " err_x: " << dx << std::endl
                         << " err_y:" << dy << std::endl
                         << " rho_error:" << rho_error << std::endl
                         << " alpha_error:" << alpha_error << std::endl
                         << " betta_error:" << betta_error << std::endl
                         << " vetta:" << vetta << std::endl
                         << " gamma:" << gamma << std::endl
                         << " xy_goal_tolerance:" << xy_goal_tolerance_ << std::endl
                         << " yaw_goal_tolerance:" << yaw_goal_tolerance_ << std::endl);
 
  // integrate trajectory and check collision
 
  assert(currentPose.header.frame_id == "odom" || currentPose.header.frame_id == "map");
  auto global_pose = currentPose;
 
  auto * costmap2d = costmapRos_->getCostmap();
  auto yaw = tf2::getYaw(global_pose.pose.orientation);
 
  auto & pos = global_pose.pose.position;
 
  Eigen::Vector3f currentpose(pos.x, pos.y, yaw);
  Eigen::Vector3f currentvel(
    cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z);
 
  std::vector<Eigen::Vector3f> trajectory;
  this->generateTrajectory(
    currentpose, currentvel, 0.8 /*meters*/, M_PI / 8 /*rads*/, 3.0 /*seconds*/, 0.05 /*seconds*/,
    trajectory);
 
  // check plan rejection
  bool aceptedplan = true;
 
  unsigned int mx, my;
 
  int i = 0;
  // RCLCPP_INFO_STREAM(nh_->get_logger(), "lplanner goal: " << finalgoalpose.pose.position);
  for (auto & p : trajectory)
  {
    float dx = p[0] - finalgoalpose.pose.position.x;
    float dy = p[1] - finalgoalpose.pose.position.y;
 
    float dst = sqrt(dx * dx + dy * dy);
    if (dst < xy_goal_tolerance_)
    {
      //  RCLCPP_INFO(nh_->get_logger(), "trajectory checking skipped, goal reached");
      break;
    }
 
    costmap2d->worldToMap(p[0], p[1], mx, my);
 
    // RCLCPP_INFO(nh_->get_logger(), "checking cost pt %d [%lf, %lf] cell[%d,%d] = %d", i, p[0], p[1], mx, my, cost);
    // RCLCPP_INFO_STREAM(nh_->get_logger(), "cost: " << cost);
 
    // static const unsigned char NO_INFORMATION = 255;
    // static const unsigned char LETHAL_OBSTACLE = 254;
    // static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
    // static const unsigned char FREE_SPACE = 0;
 
    if (costmap2d->getCost(mx, my) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
    {
      aceptedplan = false;
      // RCLCPP_WARN(nh_->get_logger(), "ABORTED LOCAL PLAN BECAUSE OBSTACLE DETEDTED");
      break;
    }
    i++;
  }
 
  bool success = false;
  if (aceptedplan)
  {
    waiting_ = false;
    success = true;
    RCLCPP_DEBUG(nh_->get_logger(), "simulated trajectory is accepted.");
  }
  else
  {
    RCLCPP_DEBUG(nh_->get_logger(), "simulated trajectory is not accepted. Stop command.");
 
    // stop and wait
    cmd_vel.twist.linear.x = 0;
    cmd_vel.twist.angular.z = 0;
 
    if (!waiting_)
    {
      RCLCPP_DEBUG(nh_->get_logger(), "Start waiting obstacle disappear");
      waiting_ = true;
      waitingStamp_ = nh_->now();
    }
    else
    {
      auto waitingduration = nh_->now() - waitingStamp_;
      RCLCPP_DEBUG(
        nh_->get_logger(), "waiting obstacle disappear, elapsed: %lf seconds",
        waitingduration.seconds());
 
      if (waitingduration > this->waitingTimeout_)
      {
        RCLCPP_WARN(
          nh_->get_logger(), "TIMEOUT waiting obstacle disappear, elapsed: %lf seconds",
          waitingduration.seconds());
        success = false;
      }
    }
  }
 
  if (!success)
  {
    RCLCPP_DEBUG(
      nh_->get_logger(),
      "[ForwardLocalPlanner] object detected waiting stopped until it disappears.");
  }
 
  cmd_vel.header.stamp = nh_->now();
  return cmd_vel;
}

References alpha_offset_, betta_offset_, carrot_distance_, costmapRos_, currentPoseIndex_, generateTrajectory(), goalReached_, k_alpha_, k_betta_, k_rho_, max_angular_z_speed_, max_linear_x_speed_, nh_, plan_, publishGoalMarker(), updateParameters(), waiting_, waitingStamp_, waitingTimeout_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Here is the call graph for this function:

configure()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::configure ( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr< tf2_ros::Buffer > tf, const std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros )

override

Definition at line 68 of file forward_local_planner.cpp.

{
  nh_ = node.lock();
  costmapRos_ = costmap_ros;
  tf_ = tf;
  name_ = name;
  k_rho_ = 1.0;
  k_alpha_ = -0.4;
  k_betta_ = -1.0;  // set to zero means that orientation is not important
 
  goalReached_ = false;
  carrot_distance_ = 0.4;
  yaw_goal_tolerance_ = -1;
  xy_goal_tolerance_ = -1;
  max_linear_x_speed_ = 1.0;
  max_angular_z_speed_ = 2.0;
 
  currentPoseIndex_ = 0;
 
  declareOrSet(nh_, name_ + ".k_rho", k_rho_);
  declareOrSet(nh_, name_ + ".k_alpha", k_alpha_);
  declareOrSet(nh_, name_ + ".k_betta", k_betta_);
  declareOrSet(nh_, name_ + ".carrot_distance", carrot_distance_);
  declareOrSet(nh_, name_ + ".yaw_goal_tolerance", yaw_goal_tolerance_);
  declareOrSet(nh_, name_ + ".xy_goal_tolerance", xy_goal_tolerance_);
  declareOrSet(nh_, name_ + ".max_linear_x_speed", max_linear_x_speed_);
  declareOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
  declareOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
 
  RCLCPP_DEBUG(
    nh_->get_logger(),
    "[ForwardLocalPlanner] max linear speed: %lf, max angular speed: %lf, k_rho: %lf, "
    "carrot_distance: "
    "%lf, ",
    max_linear_x_speed_, max_angular_z_speed_, k_rho_, carrot_distance_);
  goalMarkerPublisher_ = nh_->create_publisher<visualization_msgs::msg::MarkerArray>(
    "forward_local_planner/carrot_goal_marker", rclcpp::QoS(1));
 
  waiting_ = false;
  waitingTimeout_ = rclcpp::Duration(10s);
}

References carrot_distance_, costmapRos_, currentPoseIndex_, declareOrSet(), goalMarkerPublisher_, goalReached_, k_alpha_, k_betta_, k_rho_, max_angular_z_speed_, max_linear_x_speed_, name_, nh_, tf_, transform_tolerance_, waiting_, waitingTimeout_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Here is the call graph for this function:

deactivate()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::deactivate ( )

override

Definition at line 53 of file forward_local_planner.cpp.

{
  this->cleanMarkers();
  this->goalMarkerPublisher_->on_deactivate();
}

References cleanMarkers(), and goalMarkerPublisher_.

Here is the call graph for this function:

generateTrajectory()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::generateTrajectory ( const Eigen::Vector3f & pos, const Eigen::Vector3f & vel, float maxdist, float maxangle, float maxtime, float dt, std::vector< Eigen::Vector3f > & outtraj )

private

Definition at line 142 of file forward_local_planner.cpp.

{
  // simulate the trajectory and check for collisions, updating costs along the way
  bool end = false;
  float time = 0;
  Eigen::Vector3f currentpos = pos;
  int i = 0;
  while (!end)
  {
    // add the point to the trajectory so we can draw it later if we want
    // traj.addPoint(pos[0], pos[1], pos[2]);
 
    auto loop_vel = vel;
    // update the position of the robot using the velocities passed in
    auto newpos = computeNewPositions(currentpos, loop_vel, dt);
 
    auto dx = newpos[0] - currentpos[0];
    auto dy = newpos[1] - currentpos[1];
    float dist, angledist;
 
    // RCLCPP_DEBUG(nh_->get_logger(), "traj point %d", i);
    dist = sqrt(dx * dx + dy * dy);
    if (dist > maxdist)
    {
      end = true;
      // RCLCPP_DEBUG(nh_->get_logger(), "dist break: %f", dist);
    }
    else
    {
      // ouble from, double to
      angledist = fabs(angles::shortest_angular_distance(currentpos[2], newpos[2]));
      if (angledist > maxanglediff)
      {
        end = true;
        // RCLCPP_DEBUG(nh_->get_logger(), "angle dist break: %f", angledist);
      }
      else
      {
        outtraj.push_back(newpos);
 
        time += dt;
        if (time > maxtime)
        {
          end = true;
          // RCLCPP_DEBUG(nh_->get_logger(), "time break: %f", time);
        }
 
        // RCLCPP_DEBUG(nh_->get_logger(), "dist: %f, angledist: %f, time: %f", dist, angledist, time);
      }
    }
 
    currentpos = newpos;
    i++;
  }  // end for simulation steps
}

References computeNewPositions().

Referenced by computeVelocityCommands().

Here is the call graph for this function:

Here is the caller graph for this function:

isGoalReached()

bool cl_nav2z::forward_local_planner::ForwardLocalPlanner::isGoalReached

(

)

isGoalReached()

Definition at line 625 of file forward_local_planner.cpp.

{ return goalReached_; }

References goalReached_.

publishGoalMarker()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::publishGoalMarker ( double x, double y, double phi )

private

publishGoalMarker()

Definition at line 219 of file forward_local_planner.cpp.

{
  visualization_msgs::msg::Marker marker;
 
  marker.header.frame_id = costmapRos_->getGlobalFrameID();
  marker.header.stamp = nh_->now();
  marker.ns = "my_namespace2";
  marker.id = 0;
  marker.type = visualization_msgs::msg::Marker::ARROW;
  marker.action = visualization_msgs::msg::Marker::ADD;
  marker.pose.orientation.w = 1;
  marker.lifetime = rclcpp::Duration(1.0s);
 
  marker.scale.x = 0.1;
  marker.scale.y = 0.3;
  marker.scale.z = 0.1;
  marker.color.a = 1.0;
  marker.color.r = 0;
  marker.color.g = 0;
  marker.color.b = 1.0;
 
  geometry_msgs::msg::Point start, end;
  start.x = x;
  start.y = y;
 
  end.x = x + 0.5 * cos(phi);
  end.y = y + 0.5 * sin(phi);
 
  marker.points.push_back(start);
  marker.points.push_back(end);
 
  visualization_msgs::msg::MarkerArray ma;
  ma.markers.push_back(marker);
 
  goalMarkerPublisher_->publish(ma);
}

References costmapRos_, goalMarkerPublisher_, and nh_.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

setPlan()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::setPlan ( const nav_msgs::msg::Path & plan )

override

nav2_core setPlan - Sets the global plan

Parameters

path	The global plan

setPlan()

Definition at line 632 of file forward_local_planner.cpp.

{
  nav_msgs::msg::Path transformedPlan;
 
  rclcpp::Duration ttol = rclcpp::Duration::from_seconds(transform_tolerance_);
  // transform global plan
  for (auto & p : plan.poses)
  {
    geometry_msgs::msg::PoseStamped transformedPose;
    nav_2d_utils::transformPose(tf_, costmapRos_->getGlobalFrameID(), p, transformedPose, ttol);
    transformedPose.header.frame_id = costmapRos_->getGlobalFrameID();
    transformedPlan.poses.push_back(transformedPose);
  }
 
  plan_ = transformedPlan.poses;
  goalReached_ = false;
}

References costmapRos_, goalReached_, plan_, tf_, and transform_tolerance_.

setSpeedLimit()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::setSpeedLimit ( const double & speed_limit, const bool & percentage )

overridevirtual

Definition at line 612 of file forward_local_planner.cpp.

{
  RCLCPP_WARN_STREAM(
    nh_->get_logger(),
    "ForwardLocalPlanner::setSpeedLimit invoked. Ignored, functionality not "
    "implemented.");
}

References nh_.

updateParameters()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::updateParameters ( )

private

Definition at line 113 of file forward_local_planner.cpp.

{
  nh_->get_parameter(name_ + ".k_rho", k_rho_);
  nh_->get_parameter(name_ + ".k_alpha", k_alpha_);
  nh_->get_parameter(name_ + ".k_betta", k_betta_);
  nh_->get_parameter(name_ + ".carrot_distance", carrot_distance_);
  nh_->get_parameter(name_ + ".yaw_goal_tolerance", yaw_goal_tolerance_);
  nh_->get_parameter(name_ + ".xy_goal_tolerance", xy_goal_tolerance_);
  nh_->get_parameter(name_ + ".max_linear_x_speed", max_linear_x_speed_);
  nh_->get_parameter(name_ + ".max_angular_z_speed", max_angular_z_speed_);
  nh_->get_parameter(name_ + ".transform_tolerance", transform_tolerance_);
 
  RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardLocalPlanner.k_rho: " << k_rho_);
  RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardLocalPlanner.k_alpha: " << k_alpha_);
  RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardLocalPlanner.k_betta: " << k_betta_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[ForwardLocalPlanner.carrot_distance: " << carrot_distance_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[ForwardLocalPlanner.yaw_goal_tolerance:" << yaw_goal_tolerance_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[ForwardLocalPlanner.xy_goal_tolerance: " << xy_goal_tolerance_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[ForwardLocalPlanner.max_linear_x_speed:" << max_linear_x_speed_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[ForwardLocalPlanner.max_angular_z_speed:" << max_angular_z_speed_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[ForwardLocalPlanner.transform_tolerance:" << transform_tolerance_);
}

References carrot_distance_, k_alpha_, k_betta_, k_rho_, max_angular_z_speed_, max_linear_x_speed_, name_, nh_, transform_tolerance_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Referenced by activate(), and computeVelocityCommands().

Here is the caller graph for this function:

Member Data Documentation

alpha_offset_

const double cl_nav2z::forward_local_planner::ForwardLocalPlanner::alpha_offset_ = 0

private

Definition at line 105 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands().

betta_offset_

const double cl_nav2z::forward_local_planner::ForwardLocalPlanner::betta_offset_ = 0

private

Definition at line 106 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands().

carrot_angular_distance_

rad cl_nav2z::forward_local_planner::ForwardLocalPlanner::carrot_angular_distance_

private

Definition at line 109 of file forward_local_planner.hpp.

carrot_distance_

meter cl_nav2z::forward_local_planner::ForwardLocalPlanner::carrot_distance_

private

Definition at line 108 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

costmapRos_

std::shared_ptr<nav2_costmap_2d::Costmap2DROS> cl_nav2z::forward_local_planner::ForwardLocalPlanner::costmapRos_

private

Definition at line 94 of file forward_local_planner.hpp.

Referenced by cleanMarkers(), computeVelocityCommands(), configure(), publishGoalMarker(), and setPlan().

currentPoseIndex_

int cl_nav2z::forward_local_planner::ForwardLocalPlanner::currentPoseIndex_

private

Definition at line 119 of file forward_local_planner.hpp.

Referenced by cleanup(), computeVelocityCommands(), and configure().

goalMarkerPublisher_

rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr cl_nav2z::forward_local_planner::ForwardLocalPlanner::goalMarkerPublisher_

private

Definition at line 98 of file forward_local_planner.hpp.

Referenced by activate(), cleanMarkers(), configure(), deactivate(), and publishGoalMarker().

goalReached_

bool cl_nav2z::forward_local_planner::ForwardLocalPlanner::goalReached_

private

Definition at line 103 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), isGoalReached(), and setPlan().

k_alpha_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::k_alpha_

private

Definition at line 101 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

k_betta_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::k_betta_

private

Definition at line 102 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

k_rho_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::k_rho_

private

Definition at line 100 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

max_angular_z_speed_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::max_angular_z_speed_

private

Definition at line 114 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

max_linear_x_speed_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::max_linear_x_speed_

private

Definition at line 115 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

name_

std::string cl_nav2z::forward_local_planner::ForwardLocalPlanner::name_

private

Definition at line 95 of file forward_local_planner.hpp.

Referenced by configure(), and updateParameters().

nh_

nav2_util::LifecycleNode::SharedPtr cl_nav2z::forward_local_planner::ForwardLocalPlanner::nh_

private

Definition at line 83 of file forward_local_planner.hpp.

Referenced by activate(), cleanMarkers(), computeVelocityCommands(), configure(), publishGoalMarker(), setSpeedLimit(), and updateParameters().

plan_

std::vector<geometry_msgs::msg::PoseStamped> cl_nav2z::forward_local_planner::ForwardLocalPlanner::plan_

private

Definition at line 121 of file forward_local_planner.hpp.

Referenced by cleanup(), computeVelocityCommands(), and setPlan().

tf_

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::forward_local_planner::ForwardLocalPlanner::tf_

private

Definition at line 126 of file forward_local_planner.hpp.

Referenced by configure(), and setPlan().

transform_tolerance_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::transform_tolerance_

private

Definition at line 116 of file forward_local_planner.hpp.

Referenced by configure(), setPlan(), and updateParameters().

waiting_

bool cl_nav2z::forward_local_planner::ForwardLocalPlanner::waiting_

private

Definition at line 123 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), and configure().

waitingStamp_

rclcpp::Time cl_nav2z::forward_local_planner::ForwardLocalPlanner::waitingStamp_

private

Definition at line 125 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands().

waitingTimeout_

rclcpp::Duration cl_nav2z::forward_local_planner::ForwardLocalPlanner::waitingTimeout_

private

Definition at line 124 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), and configure().

xy_goal_tolerance_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::xy_goal_tolerance_

private

Definition at line 112 of file forward_local_planner.hpp.

Referenced by cleanup(), computeVelocityCommands(), configure(), and updateParameters().

yaw_goal_tolerance_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::yaw_goal_tolerance_

private

Definition at line 111 of file forward_local_planner.hpp.

Referenced by cleanup(), computeVelocityCommands(), configure(), and updateParameters().

---

The documentation for this class was generated from the following files:

• smacc2_client_library/cl_nav2z/custom_planners/forward_local_planner/include/forward_local_planner/forward_local_planner.hpp
• smacc2_client_library/cl_nav2z/custom_planners/forward_local_planner/src/forward_local_planner/forward_local_planner.cpp