#include <pure_spinning_local_planner.hpp>

Inheritance diagram for cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner:

Collaboration diagram for cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner:

Public Member Functions
	PureSpinningLocalPlanner ()

virtual	~PureSpinningLocalPlanner ()

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 More...

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 More...

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)

Private Attributes
nav2_util::LifecycleNode::SharedPtr	nh_

std::string	name_

std::shared_ptr< nav2_costmap_2d::Costmap2DROS >	costmapRos_

std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray > >	goalMarkerPublisher_

std::vector< geometry_msgs::msg::PoseStamped >	plan_

std::shared_ptr< tf2_ros::Buffer >	tf_

double	k_betta_

bool	goalReached_

int	currentPoseIndex_

rad	yaw_goal_tolerance_

rad	intermediate_goal_yaw_tolerance_

rad_s	max_angular_z_speed_

double	transform_tolerance_

bool	use_shortest_angular_distance_

Detailed Description

Definition at line 41 of file pure_spinning_local_planner.hpp.

Constructor & Destructor Documentation

◆ PureSpinningLocalPlanner()

cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::PureSpinningLocalPlanner ( )

Definition at line 35 of file pure_spinning_local_planner.cpp.

35{}

◆ ~PureSpinningLocalPlanner()

cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::~PureSpinningLocalPlanner ( )

virtual

Definition at line 37 of file pure_spinning_local_planner.cpp.

37{}

Member Function Documentation

◆ activate()

void cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::activate ( )

override

Definition at line 39 of file pure_spinning_local_planner.cpp.

{
  RCLCPP_INFO_STREAM(nh_->get_logger(), "activating controller PureSpinningLocalPlanner");
  this->updateParameters();
}

References nh_, and updateParameters().

Here is the call graph for this function:

◆ cleanup()

void cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::cleanup ( )

override

Definition at line 47 of file pure_spinning_local_planner.cpp.

{
  this->plan_.clear();
  this->currentPoseIndex_ = 0;
  yaw_goal_tolerance_ = -1.0;
}

References currentPoseIndex_, plan_, and yaw_goal_tolerance_.

◆ computeVelocityCommands()

geometry_msgs::msg::TwistStamped cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::computeVelocityCommands	(	const geometry_msgs::msg::PoseStamped &	pose,
		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

Definition at line 88 of file pure_spinning_local_planner.cpp.

{
  this->updateParameters();
  if (yaw_goal_tolerance_ == -1)
  {
    geometry_msgs::msg::Pose posetol;
    geometry_msgs::msg::Twist twistol;
    if (goal_checker->getTolerances(posetol, twistol))
    {
      yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation);
      //yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation) * 0.35; // WORKAROUND GOAL CHECKER DIFFERENCE NAV CONTROLLER
      RCLCPP_INFO_STREAM(
        nh_->get_logger(),
        "[PureSpinningLocalPlanner] yaw_goal_tolerance_: " << yaw_goal_tolerance_);
    }
    else
    {
      RCLCPP_INFO_STREAM(
        nh_->get_logger(), "[PureSpinningLocalPlanner] could not get tolerances from goal checker");
    }
  }
 
  geometry_msgs::msg::TwistStamped cmd_vel;
 
  goalReached_ = false;
  // RCLCPP_DEBUG(nh_->get_logger(), "LOCAL PLANNER LOOP");
 
  geometry_msgs::msg::PoseStamped currentPose = pose;
 
  tf2::Quaternion q;
  tf2::fromMsg(currentPose.pose.orientation, q);
  auto currentYaw = tf2::getYaw(currentPose.pose.orientation);
  double angular_error = 0;
  double targetYaw;
 
  do
  {
    auto & goal = plan_[currentPoseIndex_];
    targetYaw = tf2::getYaw(goal.pose.orientation);
 
    if (use_shortest_angular_distance_)
      angular_error = angles::shortest_angular_distance(currentYaw, targetYaw);
    else
      angular_error = targetYaw - currentYaw;
 
    // if it is in the following way, sometimes the direction is incorrect
    //angular_error = targetYaw - currentYaw;
 
    // all the points must be reached using the control rule, but the last one
    // have an special condition
    if ((currentPoseIndex_ < (int)plan_.size() - 1 &&
         fabs(angular_error) < this->intermediate_goal_yaw_tolerance_))
    {
      currentPoseIndex_++;
    }
    else
    {
      break;
    }
  } while (currentPoseIndex_ < (int)plan_.size());
 
  auto omega = angular_error * k_betta_;
  cmd_vel.twist.angular.z =
    std::min(std::max(omega, -fabs(max_angular_z_speed_)), fabs(max_angular_z_speed_));
 
  RCLCPP_INFO_STREAM(
    nh_->get_logger(),
    "[PureSpinningLocalPlanner] pose index: " << currentPoseIndex_ << "/" << plan_.size());
  RCLCPP_INFO_STREAM(nh_->get_logger(), "[PureSpinningLocalPlanner] k_betta_: " << k_betta_);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[PureSpinningLocalPlanner] angular error: " << angular_error << "(tol "
                                                                    << yaw_goal_tolerance_ << ")");
  RCLCPP_INFO_STREAM(
    nh_->get_logger(),
    "[PureSpinningLocalPlanner] command angular speed: " << cmd_vel.twist.angular.z);
  RCLCPP_INFO_STREAM(
    nh_->get_logger(),
    "[PureSpinningLocalPlanner] completion " << currentPoseIndex_ << "/" << plan_.size());
 
  if (currentPoseIndex_ >= (int)plan_.size() - 1 && fabs(angular_error) < yaw_goal_tolerance_)
  {
    RCLCPP_INFO_STREAM(
      nh_->get_logger(), "[PureSpinningLocalPlanner] GOAL REACHED. Sending stop command.");
    goalReached_ = true;
    cmd_vel.twist.linear.x = 0;
    cmd_vel.twist.angular.z = 0;
  }
 
  return cmd_vel;
}

References currentPoseIndex_, goalReached_, intermediate_goal_yaw_tolerance_, k_betta_, max_angular_z_speed_, nh_, plan_, updateParameters(), use_shortest_angular_distance_, and yaw_goal_tolerance_.

Here is the call graph for this function:

◆ configure()

void cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::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 54 of file pure_spinning_local_planner.cpp.

{
  costmapRos_ = costmap_ros;
  name_ = name;
  nh_ = node.lock();
  k_betta_ = 1.0;
  intermediate_goal_yaw_tolerance_ = 0.12;
  max_angular_z_speed_ = 1.0;
  yaw_goal_tolerance_ = -1.0;
  transform_tolerance_ = 0.1;
  tf_ = tf;
 
  declareOrSet(nh_, name_ + ".k_betta", k_betta_);
  declareOrSet(nh_, name_ + ".intermediate_goals_yaw_tolerance", intermediate_goal_yaw_tolerance_);
  declareOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
  declareOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
  declareOrSet(nh_, name_ + ".use_shortest_angular_distance", use_shortest_angular_distance_);
 
  RCLCPP_INFO_STREAM(
    nh_->get_logger(), "[PureSpinningLocalPlanner] pure spinning planner already created");
}

References costmapRos_, declareOrSet(), intermediate_goal_yaw_tolerance_, k_betta_, max_angular_z_speed_, name_, nh_, tf_, transform_tolerance_, use_shortest_angular_distance_, and yaw_goal_tolerance_.

Here is the call graph for this function:

◆ deactivate()

void cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::deactivate ( )

override

Definition at line 45 of file pure_spinning_local_planner.cpp.

45{}

◆ isGoalReached()

bool cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::isGoalReached ( )

Definition at line 190 of file pure_spinning_local_planner.cpp.

190{ return goalReached_; }

References goalReached_.

◆ publishGoalMarker()

void cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::publishGoalMarker	(	double	x,
		double	y,
		double	phi
	)

private

◆ setPlan()

void cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::setPlan ( const nav_msgs::msg::Path & path )

override

nav2_core setPlan - Sets the global plan

Parameters

path	The global plan

Definition at line 192 of file pure_spinning_local_planner.cpp.

{
  RCLCPP_INFO_STREAM(nh_->get_logger(), "activating controller PureSpinningLocalPlanner");
 
  nav_msgs::msg::Path transformedPlan;
 
  rclcpp::Duration ttol = rclcpp::Duration::from_seconds(transform_tolerance_);
  // transform global plan
  for (auto & p : path.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;
  currentPoseIndex_ = 0;
}

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

◆ setSpeedLimit()

void cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::setSpeedLimit	(	const double &	speed_limit,
		const bool &	percentage
	)

overridevirtual

Definition at line 181 of file pure_spinning_local_planner.cpp.

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

References nh_.

◆ updateParameters()

void cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::updateParameters ( )

private

Definition at line 79 of file pure_spinning_local_planner.cpp.

{
  tryGetOrSet(nh_, name_ + ".k_betta", k_betta_);
  tryGetOrSet(nh_, name_ + ".intermediate_goals_yaw_tolerance", intermediate_goal_yaw_tolerance_);
  tryGetOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
  tryGetOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
  tryGetOrSet(nh_, name_ + ".use_shortest_angular_distance", use_shortest_angular_distance_);
}

References intermediate_goal_yaw_tolerance_, k_betta_, max_angular_z_speed_, name_, nh_, transform_tolerance_, cl_nav2z::pure_spinning_local_planner::tryGetOrSet(), and use_shortest_angular_distance_.

Referenced by activate(), and computeVelocityCommands().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ costmapRos_

std::shared_ptr<nav2_costmap_2d::Costmap2DROS> cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::costmapRos_

private

Definition at line 90 of file pure_spinning_local_planner.hpp.

Referenced by configure(), and setPlan().

◆ currentPoseIndex_

int cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::currentPoseIndex_

private

Definition at line 101 of file pure_spinning_local_planner.hpp.

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

◆ goalMarkerPublisher_

std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray> > cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::goalMarkerPublisher_

private

Definition at line 93 of file pure_spinning_local_planner.hpp.

◆ goalReached_

bool cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::goalReached_

private

Definition at line 100 of file pure_spinning_local_planner.hpp.

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

◆ intermediate_goal_yaw_tolerance_

rad cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::intermediate_goal_yaw_tolerance_

private

Definition at line 103 of file pure_spinning_local_planner.hpp.

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

◆ k_betta_

double cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::k_betta_

private

Definition at line 99 of file pure_spinning_local_planner.hpp.

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

◆ max_angular_z_speed_

rad_s cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::max_angular_z_speed_

private

Definition at line 104 of file pure_spinning_local_planner.hpp.

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

◆ name_

std::string cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::name_

private

Definition at line 87 of file pure_spinning_local_planner.hpp.

Referenced by configure(), and updateParameters().

◆ nh_

nav2_util::LifecycleNode::SharedPtr cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::nh_

private

Definition at line 86 of file pure_spinning_local_planner.hpp.

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

◆ plan_

std::vector<geometry_msgs::msg::PoseStamped> cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::plan_

private

Definition at line 95 of file pure_spinning_local_planner.hpp.

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

◆ tf_

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::tf_

private

Definition at line 97 of file pure_spinning_local_planner.hpp.

Referenced by configure(), and setPlan().

◆ transform_tolerance_

double cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::transform_tolerance_

private

Definition at line 105 of file pure_spinning_local_planner.hpp.

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

◆ use_shortest_angular_distance_

bool cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::use_shortest_angular_distance_

private

Definition at line 106 of file pure_spinning_local_planner.hpp.

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

◆ yaw_goal_tolerance_

rad cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::yaw_goal_tolerance_

private

Definition at line 102 of file pure_spinning_local_planner.hpp.

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

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

smacc2_client_library/nav2z_client/custom_planners/pure_spinning_local_planner/include/pure_spinning_local_planner/pure_spinning_local_planner.hpp
smacc2_client_library/nav2z_client/custom_planners/pure_spinning_local_planner/src/pure_spinning_local_planner/pure_spinning_local_planner.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PureSpinningLocalPlanner()

◆ ~PureSpinningLocalPlanner()

Member Function Documentation

◆ activate()

◆ cleanup()

◆ computeVelocityCommands()

◆ configure()

◆ deactivate()

◆ isGoalReached()

◆ publishGoalMarker()

◆ setPlan()

◆ setSpeedLimit()

◆ updateParameters()

Member Data Documentation

◆ costmapRos_

◆ currentPoseIndex_

◆ goalMarkerPublisher_

◆ goalReached_

◆ intermediate_goal_yaw_tolerance_

◆ k_betta_

◆ max_angular_z_speed_

◆ name_

◆ nh_

◆ plan_

◆ tf_

◆ transform_tolerance_

◆ use_shortest_angular_distance_

◆ yaw_goal_tolerance_