#include <pure_spinning_local_planner.h>

Inheritance diagram for cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner:

Collaboration diagram for cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner:

Public Member Functions
	PureSpinningLocalPlanner ()

virtual	~PureSpinningLocalPlanner ()

virtual bool	computeVelocityCommands (geometry_msgs::Twist &cmd_vel) override
	Given the current position, orientation, and velocity of the robot: compute velocity commands to send to the robot mobile base. More...

virtual bool	isGoalReached () override
	Check if the goal pose has been achieved by the local planner. More...

virtual bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &plan) override
	Set the plan that the local planner is following. More...

void	initialize (std::string name, tf::TransformListener tf, costmap_2d::Costmap2DROS costmapRos)
	Constructs the local planner. More...

void	initialize (std::string name, tf2_ros::Buffer tf, costmap_2d::Costmap2DROS costmapRos)

void	initialize ()

Private Member Functions
void	reconfigCB (::pure_spinning_local_planner::PureSpinningLocalPlannerConfig &config, uint32_t level)

void	publishGoalMarker (double x, double y, double phi)

Private Attributes
costmap_2d::Costmap2DROS *	costmapRos_

std::vector< geometry_msgs::PoseStamped >	plan_

dynamic_reconfigure::Server<::pure_spinning_local_planner::PureSpinningLocalPlannerConfig >	paramServer_

double	k_betta_

bool	goalReached_

int	currentPoseIndex_

rad	yaw_goal_tolerance_

rad	intermediate_goal_yaw_tolerance_

rad_s	max_angular_z_speed_

Detailed Description

Definition at line 24 of file pure_spinning_local_planner.h.

Constructor & Destructor Documentation

◆ PureSpinningLocalPlanner()

cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::PureSpinningLocalPlanner ( )

Definition at line 32 of file pure_spinning_local_planner.cpp.

  : paramServer_(ros::NodeHandle("~PureSpinningLocalPlanner"))
{
  ROS_INFO_STREAM("[PureSpinningLocalPlanner] pure spinning planner already created");
}

◆ ~PureSpinningLocalPlanner()

cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::~PureSpinningLocalPlanner ( )

virtual

Definition at line 38 of file pure_spinning_local_planner.cpp.

39{

40}

Member Function Documentation

◆ computeVelocityCommands()

bool cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::computeVelocityCommands ( geometry_msgs::Twist & cmd_vel )

overridevirtual

Given the current position, orientation, and velocity of the robot: compute velocity commands to send to the robot mobile base.

Parameters

cmd_vel Will be filled with the velocity command to be passed to the robot base

Returns: True if a valid velocity command was found, false otherwise

Definition at line 74 of file pure_spinning_local_planner.cpp.

{
  goalReached_ = false;
  // ROS_DEBUG("LOCAL PLANNER LOOP");
 
  tf::Stamped<tf::Pose> tfpose = optionalRobotPose(costmapRos_);
 
  geometry_msgs::PoseStamped currentPose;
  tf::poseStampedTFToMsg(tfpose, currentPose);
  // ROS_INFO_STREAM("[PureSpinningLocalPlanner] current robot pose " << currentPose);
 
  tf::Quaternion q = tfpose.getRotation();
  auto currentYaw = tf::getYaw(currentPose.pose.orientation);
  double angular_error;
  double targetYaw;
 
  while (currentPoseIndex_ < plan_.size())
  {
    auto &goal = plan_[currentPoseIndex_];
    targetYaw = tf::getYaw(goal.pose.orientation);
 
    //double angular_error = angles::shortest_angular_distance( currentYaw , targetYaw) ;
    angular_error = targetYaw - currentYaw ;
 
    // all the points must be reached using the control rule, but the last one
    // have an special condition
    if ( (currentPoseIndex_ < plan_.size() -1 && fabs(angular_error) < this->intermediate_goal_yaw_tolerance_)
         || (currentPoseIndex_ == plan_.size() -1 && fabs(angular_error) < this->intermediate_goal_yaw_tolerance_)
    )
    {
      currentPoseIndex_++;
    }
    else
    {
      break;
    }
  }
 
  auto omega = angular_error * k_betta_;
  cmd_vel.angular.z = std::min(std::max(omega, -fabs(max_angular_z_speed_)), fabs(max_angular_z_speed_));
 
  //ROS_DEBUG_STREAM("[PureSpinningLocalPlanner] current yaw: " << currentYaw);
  //ROS_DEBUG_STREAM("[PureSpinningLocalPlanner] target yaw: " << targetYaw);
  ROS_DEBUG_STREAM("[PureSpinningLocalPlanner] angular error: " << angular_error << "("<<yaw_goal_tolerance_<<")");
  ROS_DEBUG_STREAM("[PureSpinningLocalPlanner] command angular speed: " << cmd_vel.angular.z);
  ROS_DEBUG_STREAM("[PureSpinningLocalPlanner] completion " << currentPoseIndex_ << "/"<< plan_.size());
 
  if (currentPoseIndex_ >= plan_.size() -1 && fabs(angular_error) < yaw_goal_tolerance_)
  {
    ROS_DEBUG_STREAM("[PureSpinningLocalPlanner] GOAL REACHED ");
    goalReached_ = true;
  }
 
  return true;
}

References costmapRos_, currentPoseIndex_, goalReached_, intermediate_goal_yaw_tolerance_, k_betta_, max_angular_z_speed_, cl_move_base_z::pure_spinning_local_planner::optionalRobotPose(), plan_, and yaw_goal_tolerance_.

Here is the call graph for this function:

◆ initialize() [1/3]

void cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::initialize ( )

Definition at line 55 of file pure_spinning_local_planner.cpp.

{
  ros::NodeHandle nh("~/PureSpinningLocalPlanner");
  nh.param("k_betta", k_betta_, 10.0);
  nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.08);
  nh.param("intermediate_goals_yaw_tolerance", intermediate_goal_yaw_tolerance_, 0.12);
  nh.param("max_angular_z_speed", max_angular_z_speed_, 1.0);
 
  dynamic_reconfigure::Server<::pure_spinning_local_planner::PureSpinningLocalPlannerConfig>::CallbackType f;
  f = boost::bind(&PureSpinningLocalPlanner::reconfigCB, this, _1, _2);
  paramServer_.setCallback(f);
}

References intermediate_goal_yaw_tolerance_, k_betta_, max_angular_z_speed_, paramServer_, reconfigCB(), and yaw_goal_tolerance_.

Referenced by initialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initialize() [2/3]

void cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::initialize	(	std::string	name,
		tf2_ros::Buffer *	tf,
		costmap_2d::Costmap2DROS *	costmapRos
	)

Definition at line 42 of file pure_spinning_local_planner.cpp.

{
  costmapRos_ = costmapRos;
  this->initialize();
}

References costmapRos_, and initialize().

Here is the call graph for this function:

◆ initialize() [3/3]

void cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::initialize	(	std::string	name,
		tf::TransformListener *	tf,
		costmap_2d::Costmap2DROS *	costmapRos
	)

Constructs the local planner.

Parameters

name	The name to give this instance of the local planner
tf	A pointer to a transform listener
costmap_ros	The cost map to use for assigning costs to local plans

Definition at line 48 of file pure_spinning_local_planner.cpp.

{
  costmapRos_ = costmapRos;
  this->initialize();
}

References costmapRos_, and initialize().

Here is the call graph for this function:

◆ isGoalReached()

bool cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::isGoalReached ( )

overridevirtual

Check if the goal pose has been achieved by the local planner.

Returns: True if achieved, false otherwise

Definition at line 130 of file pure_spinning_local_planner.cpp.

{
  return goalReached_;
}

References goalReached_.

◆ publishGoalMarker()

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

private

◆ reconfigCB()

void cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::reconfigCB	(	::pure_spinning_local_planner::PureSpinningLocalPlannerConfig &	config,
		uint32_t	level
	)

private

Definition at line 68 of file pure_spinning_local_planner.cpp.

{
  this->k_betta_ = config.k_betta;
  this->yaw_goal_tolerance_ = config.yaw_goal_tolerance;
  this->max_angular_z_speed_ = config.max_angular_z_speed;
}

References k_betta_, max_angular_z_speed_, and yaw_goal_tolerance_.

Referenced by initialize().

Here is the caller graph for this function:

◆ setPlan()

bool cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::setPlan ( const std::vector< geometry_msgs::PoseStamped > & plan )

overridevirtual

Set the plan that the local planner is following.

Parameters

plan	The plan to pass to the local planner

Returns: True if the plan was updated successfully, false otherwise

Definition at line 135 of file pure_spinning_local_planner.cpp.

{
  ROS_DEBUG_STREAM("[PureSpinningLocalPlanner] setting global plan to follow");
 
  plan_ = plan;
  goalReached_ = false;
  currentPoseIndex_ = 0;
  return true;
}

References currentPoseIndex_, goalReached_, and plan_.

Member Data Documentation

◆ costmapRos_

costmap_2d::Costmap2DROS* cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::costmapRos_

private

Definition at line 69 of file pure_spinning_local_planner.h.

Referenced by computeVelocityCommands(), and initialize().

◆ currentPoseIndex_

int cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::currentPoseIndex_

private

Definition at line 78 of file pure_spinning_local_planner.h.

Referenced by computeVelocityCommands(), and setPlan().

◆ goalReached_

bool cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::goalReached_

private

Definition at line 77 of file pure_spinning_local_planner.h.

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

◆ intermediate_goal_yaw_tolerance_

rad cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::intermediate_goal_yaw_tolerance_

private

Definition at line 80 of file pure_spinning_local_planner.h.

Referenced by computeVelocityCommands(), and initialize().

◆ k_betta_

double cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::k_betta_

private

Definition at line 76 of file pure_spinning_local_planner.h.

Referenced by computeVelocityCommands(), initialize(), and reconfigCB().

◆ max_angular_z_speed_

rad_s cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::max_angular_z_speed_

private

Definition at line 81 of file pure_spinning_local_planner.h.

Referenced by computeVelocityCommands(), initialize(), and reconfigCB().

◆ paramServer_

dynamic_reconfigure::Server<::pure_spinning_local_planner::PureSpinningLocalPlannerConfig> cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::paramServer_

private

Definition at line 73 of file pure_spinning_local_planner.h.

Referenced by initialize().

◆ plan_

std::vector<geometry_msgs::PoseStamped> cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::plan_

private

Definition at line 71 of file pure_spinning_local_planner.h.

Referenced by computeVelocityCommands(), and setPlan().

◆ yaw_goal_tolerance_

rad cl_move_base_z::pure_spinning_local_planner::PureSpinningLocalPlanner::yaw_goal_tolerance_

private

Definition at line 79 of file pure_spinning_local_planner.h.

Referenced by computeVelocityCommands(), initialize(), and reconfigCB().

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

smacc_client_library/move_base_z_client/custom_planners/pure_spinning_local_planner/include/pure_spinning_local_planner/pure_spinning_local_planner.h
smacc_client_library/move_base_z_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

◆ computeVelocityCommands()

◆ initialize() [1/3]

◆ initialize() [2/3]

◆ initialize() [3/3]

◆ isGoalReached()

◆ publishGoalMarker()

◆ reconfigCB()

◆ setPlan()

Member Data Documentation

◆ costmapRos_

◆ currentPoseIndex_

◆ goalReached_

◆ intermediate_goal_yaw_tolerance_

◆ k_betta_

◆ max_angular_z_speed_

◆ paramServer_

◆ plan_

◆ yaw_goal_tolerance_