SMACC2
forward_local_planner.hpp
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/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20#pragma once
21
22#include <eigen3/Eigen/Eigen>
23
24//#include <dynamic_reconfigure/server.h>
25#include <tf2/transform_datatypes.h>
27#include <visualization_msgs/msg/marker_array.hpp>
28
29#include <tf2/utils.h>
30#include <nav2_core/controller.hpp>
31
32typedef double meter;
33typedef double rad;
34
35namespace cl_nav2z
36{
37namespace forward_local_planner
38{
39class ForwardLocalPlanner : public nav2_core::Controller
40{
41public:
43
44 virtual ~ForwardLocalPlanner();
45
46 void configure(
47 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name,
48 const std::shared_ptr<tf2_ros::Buffer> & tf,
49 const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
50
51 void activate() override;
52 void deactivate() override;
53 void cleanup() override;
54
59 void setPlan(const nav_msgs::msg::Path & path) override;
60
73 virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(
74 const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity,
75 nav2_core::GoalChecker * goal_checker) override;
76
77 /*deprecated in navigation2*/
78 bool isGoalReached();
79
80 virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
81
82private:
83 void updateParameters();
84 nav2_util::LifecycleNode::SharedPtr nh_;
85
86 void publishGoalMarker(double x, double y, double phi);
87 void cleanMarkers();
88
90 const Eigen::Vector3f & pos, const Eigen::Vector3f & vel, float maxdist, float maxangle,
91 float maxtime, float dt, std::vector<Eigen::Vector3f> & outtraj);
92 Eigen::Vector3f computeNewPositions(
93 const Eigen::Vector3f & pos, const Eigen::Vector3f & vel, double dt);
94
95 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmapRos_;
96 std::string name_;
97
98 rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
100
101 double k_rho_;
102 double k_alpha_;
103 double k_betta_;
105
106 const double alpha_offset_ = 0;
107 const double betta_offset_ = 0;
108
111
112 double yaw_goal_tolerance_; // radians
113 double xy_goal_tolerance_; // meters
114
118
119 // references the current point inside the backwardsPlanPath were the robot is located
121
122 std::vector<geometry_msgs::msg::PoseStamped> plan_;
123
125 rclcpp::Duration waitingTimeout_;
126 rclcpp::Time waitingStamp_;
127 std::shared_ptr<tf2_ros::Buffer> tf_;
128};
129} // namespace forward_local_planner
130} // namespace cl_nav2z
double rad
double meter
std::vector< geometry_msgs::msg::PoseStamped > plan_
std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmapRos_
Eigen::Vector3f computeNewPositions(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)
void generateTrajectory(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, float maxdist, float maxangle, float maxtime, float dt, std::vector< Eigen::Vector3f > &outtraj)
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
rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray >::SharedPtr goalMarkerPublisher_
double rad
double meter