SMACC2
pure_spinning_local_planner.cpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15#include <angles/angles.h>
18
19#include <nav_2d_utils/tf_help.hpp>
20#include <pluginlib/class_list_macros.hpp>
21
22namespace cl_nav2z
23{
24namespace pure_spinning_local_planner
25{
26template <typename T>
27void tryGetOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr & node, std::string param, T & value)
28{
29 if (!node->get_parameter(param, value))
30 {
31 node->set_parameter(rclcpp::Parameter(param, value));
32 }
33}
34
36
38
40{
41 RCLCPP_INFO_STREAM(nh_->get_logger(), "activating controller PureSpinningLocalPlanner");
42 this->updateParameters();
43}
44
46
48{
49 this->plan_.clear();
50 this->currentPoseIndex_ = 0;
52}
53
55 const rclcpp_lifecycle::LifecycleNode::WeakPtr & node, std::string name,
56 const std::shared_ptr<tf2_ros::Buffer> & tf,
57 const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros)
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}
78
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}
87
89 const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & /*velocity*/,
90 nav2_core::GoalChecker * goal_checker)
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}
180
182 const double & /*speed_limit*/, const bool & /*percentage*/)
183{
184 RCLCPP_WARN_STREAM(
185 nh_->get_logger(),
186 "PureSpinningLocalPlanner::setSpeedLimit invoked. Ignored, funcionality not "
187 "implemented.");
188}
189
191
192void PureSpinningLocalPlanner::setPlan(const nav_msgs::msg::Path & path)
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}
213
214void publishGoalMarker(double /*x*/, double /*y*/, double /*phi*/) {}
215} // namespace pure_spinning_local_planner
216} // namespace cl_nav2z
217
218PLUGINLIB_EXPORT_CLASS(
virtual void setSpeedLimit(const double &speed_limit, const bool &percentage) override
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
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
void setPlan(const nav_msgs::msg::Path &path) override
nav2_core setPlan - Sets the global plan
void declareOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)
Definition: common.hpp:34
void tryGetOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)