SMACC2
Loading...
Searching...
No Matches
Public Member Functions | Private Member Functions | Private Attributes | List of all members
cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner Class Reference

#include <pure_spinning_local_planner.hpp>

Inheritance diagram for cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner:
Inheritance graph
Collaboration diagram for cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner:
Collaboration graph

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.

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

References nh_, and updateParameters().

Here is the call graph for this function:

◆ cleanup()

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

◆ 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
poseCurrent robot pose
velocityCurrent robot velocity
Returns
The best command for the robot to drive

Definition at line 88 of file pure_spinning_local_planner.cpp.

91{
92 this->updateParameters();
93 if (yaw_goal_tolerance_ == -1)
94 {
95 geometry_msgs::msg::Pose posetol;
96 geometry_msgs::msg::Twist twistol;
97 if (goal_checker->getTolerances(posetol, twistol))
98 {
99 yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation);
100 //yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation) * 0.35; // WORKAROUND GOAL CHECKER DIFFERENCE NAV CONTROLLER
101 RCLCPP_INFO_STREAM(
102 nh_->get_logger(),
103 "[PureSpinningLocalPlanner] yaw_goal_tolerance_: " << yaw_goal_tolerance_);
104 }
105 else
106 {
107 RCLCPP_INFO_STREAM(
108 nh_->get_logger(), "[PureSpinningLocalPlanner] could not get tolerances from goal checker");
109 }
110 }
111
112 geometry_msgs::msg::TwistStamped cmd_vel;
113
114 goalReached_ = false;
115 // RCLCPP_DEBUG(nh_->get_logger(), "LOCAL PLANNER LOOP");
116
117 geometry_msgs::msg::PoseStamped currentPose = pose;
118
119 tf2::Quaternion q;
120 tf2::fromMsg(currentPose.pose.orientation, q);
121 auto currentYaw = tf2::getYaw(currentPose.pose.orientation);
122 double angular_error = 0;
123 double targetYaw;
124
125 do
126 {
127 auto & goal = plan_[currentPoseIndex_];
128 targetYaw = tf2::getYaw(goal.pose.orientation);
129
131 angular_error = angles::shortest_angular_distance(currentYaw, targetYaw);
132 else
133 angular_error = targetYaw - currentYaw;
134
135 // if it is in the following way, sometimes the direction is incorrect
136 //angular_error = targetYaw - currentYaw;
137
138 // all the points must be reached using the control rule, but the last one
139 // have an special condition
140 if ((currentPoseIndex_ < (int)plan_.size() - 1 &&
141 fabs(angular_error) < this->intermediate_goal_yaw_tolerance_))
142 {
144 }
145 else
146 {
147 break;
148 }
149 } while (currentPoseIndex_ < (int)plan_.size());
150
151 auto omega = angular_error * k_betta_;
152 cmd_vel.twist.angular.z =
153 std::min(std::max(omega, -fabs(max_angular_z_speed_)), fabs(max_angular_z_speed_));
154
155 RCLCPP_INFO_STREAM(
156 nh_->get_logger(),
157 "[PureSpinningLocalPlanner] pose index: " << currentPoseIndex_ << "/" << plan_.size());
158 RCLCPP_INFO_STREAM(nh_->get_logger(), "[PureSpinningLocalPlanner] k_betta_: " << k_betta_);
159 RCLCPP_INFO_STREAM(
160 nh_->get_logger(), "[PureSpinningLocalPlanner] angular error: " << angular_error << "(tol "
161 << yaw_goal_tolerance_ << ")");
162 RCLCPP_INFO_STREAM(
163 nh_->get_logger(),
164 "[PureSpinningLocalPlanner] command angular speed: " << cmd_vel.twist.angular.z);
165 RCLCPP_INFO_STREAM(
166 nh_->get_logger(),
167 "[PureSpinningLocalPlanner] completion " << currentPoseIndex_ << "/" << plan_.size());
168
169 if (currentPoseIndex_ >= (int)plan_.size() - 1 && fabs(angular_error) < yaw_goal_tolerance_)
170 {
171 RCLCPP_INFO_STREAM(
172 nh_->get_logger(), "[PureSpinningLocalPlanner] GOAL REACHED. Sending stop command.");
173 goalReached_ = true;
174 cmd_vel.twist.linear.x = 0;
175 cmd_vel.twist.angular.z = 0;
176 }
177
178 return cmd_vel;
179}

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.

58{
59 costmapRos_ = costmap_ros;
60 name_ = name;
61 nh_ = node.lock();
62 k_betta_ = 1.0;
67 tf_ = tf;
68
69 declareOrSet(nh_, name_ + ".k_betta", k_betta_);
70 declareOrSet(nh_, name_ + ".intermediate_goals_yaw_tolerance", intermediate_goal_yaw_tolerance_);
71 declareOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
72 declareOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
73 declareOrSet(nh_, name_ + ".use_shortest_angular_distance", use_shortest_angular_distance_);
74
75 RCLCPP_INFO_STREAM(
76 nh_->get_logger(), "[PureSpinningLocalPlanner] pure spinning planner already created");
77}
void declareOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)
Definition: common.hpp:34

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
pathThe global plan

Definition at line 192 of file pure_spinning_local_planner.cpp.

193{
194 RCLCPP_INFO_STREAM(nh_->get_logger(), "activating controller PureSpinningLocalPlanner");
195
196 nav_msgs::msg::Path transformedPlan;
197
198 rclcpp::Duration ttol = rclcpp::Duration::from_seconds(transform_tolerance_);
199 // transform global plan
200 for (auto & p : path.poses)
201 {
202 geometry_msgs::msg::PoseStamped transformedPose;
203 nav_2d_utils::transformPose(tf_, costmapRos_->getGlobalFrameID(), p, transformedPose, ttol);
204 transformedPose.header.frame_id = costmapRos_->getGlobalFrameID();
205 transformedPlan.poses.push_back(transformedPose);
206 }
207
208 plan_ = transformedPlan.poses;
209
210 goalReached_ = false;
212}

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.

183{
184 RCLCPP_WARN_STREAM(
185 nh_->get_logger(),
186 "PureSpinningLocalPlanner::setSpeedLimit invoked. Ignored, funcionality not "
187 "implemented.");
188}

References nh_.

◆ updateParameters()

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

Definition at line 79 of file pure_spinning_local_planner.cpp.

80{
81 tryGetOrSet(nh_, name_ + ".k_betta", k_betta_);
82 tryGetOrSet(nh_, name_ + ".intermediate_goals_yaw_tolerance", intermediate_goal_yaw_tolerance_);
83 tryGetOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
84 tryGetOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
85 tryGetOrSet(nh_, name_ + ".use_shortest_angular_distance", use_shortest_angular_distance_);
86}
void tryGetOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)

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

◆ intermediate_goal_yaw_tolerance_

rad cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::intermediate_goal_yaw_tolerance_
private

◆ k_betta_

double cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::k_betta_
private

◆ max_angular_z_speed_

rad_s cl_nav2z::pure_spinning_local_planner::PureSpinningLocalPlanner::max_angular_z_speed_
private

◆ 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

◆ 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

◆ 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: