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

#include <backward_global_planner.hpp>

Inheritance diagram for cl_nav2z::backward_global_planner::BackwardGlobalPlanner:
Inheritance graph
Collaboration diagram for cl_nav2z::backward_global_planner::BackwardGlobalPlanner:
Collaboration graph

Public Member Functions

 BackwardGlobalPlanner ()
 
virtual ~BackwardGlobalPlanner ()
 Virtual destructor. More...
 
virtual void configure (const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, std::shared_ptr< tf2_ros::Buffer > tf, std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros)
 
virtual void cleanup ()
 Method to cleanup resources used on shutdown. More...
 
virtual void activate ()
 Method to active planner and any threads involved in execution. More...
 
virtual void deactivate ()
 Method to deactivate planner and any threads involved in execution. More...
 
virtual nav_msgs::msg::Path createPlan (const geometry_msgs::msg::PoseStamped &start, const geometry_msgs::msg::PoseStamped &goal)
 Method create the plan from a starting and ending goal. More...
 

Private Member Functions

void onForwardTrailMsg (const nav_msgs::msg::Path::ConstSharedPtr &trailMessage)
 
void publishGoalMarker (const geometry_msgs::msg::Pose &pose, double r, double g, double b)
 
void cleanMarkers ()
 
void createDefaultBackwardPath (const geometry_msgs::msg::PoseStamped &start, const geometry_msgs::msg::PoseStamped &goal, std::vector< geometry_msgs::msg::PoseStamped > &plan)
 

Private Attributes

rclcpp_lifecycle::LifecycleNode::SharedPtr nh_
 
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::Path > > planPub_
 
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray > > markersPub_
 
std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros_
 
double skip_straight_motion_distance_
 
double puresSpinningRadStep_
 
std::string name_
 
std::shared_ptr< tf2_ros::Buffer > tf_
 
double transform_tolerance_
 

Detailed Description

Definition at line 31 of file backward_global_planner.hpp.

Constructor & Destructor Documentation

◆ BackwardGlobalPlanner()

cl_nav2z::backward_global_planner::BackwardGlobalPlanner::BackwardGlobalPlanner ( )

◆ ~BackwardGlobalPlanner()

cl_nav2z::backward_global_planner::BackwardGlobalPlanner::~BackwardGlobalPlanner ( )
virtual

Virtual destructor.

Definition at line 55 of file backward_global_planner.cpp.

55{}

Member Function Documentation

◆ activate()

void cl_nav2z::backward_global_planner::BackwardGlobalPlanner::activate ( )
virtual

Method to active planner and any threads involved in execution.

activate()

Definition at line 95 of file backward_global_planner.cpp.

96{
97 RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner] activating planner");
98 planPub_->on_activate();
99 markersPub_->on_activate();
100}
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< nav_msgs::msg::Path > > planPub_
std::shared_ptr< rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray > > markersPub_

References markersPub_, nh_, and planPub_.

◆ cleanMarkers()

void cl_nav2z::backward_global_planner::BackwardGlobalPlanner::cleanMarkers ( )
private

Definition at line 162 of file backward_global_planner.cpp.

163{
164 visualization_msgs::msg::Marker marker;
165 marker.header.frame_id = this->costmap_ros_->getGlobalFrameID();
166 marker.header.stamp = nh_->now();
167 marker.ns = "my_namespace2";
168 marker.id = 0;
169 marker.action = visualization_msgs::msg::Marker::DELETEALL;
170
171 visualization_msgs::msg::MarkerArray ma;
172 ma.markers.push_back(marker);
173
174 markersPub_->publish(ma);
175}
std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros_

References costmap_ros_, markersPub_, and nh_.

Referenced by cleanup(), and deactivate().

Here is the caller graph for this function:

◆ cleanup()

void cl_nav2z::backward_global_planner::BackwardGlobalPlanner::cleanup ( )
virtual

Method to cleanup resources used on shutdown.

cleanup()

Definition at line 88 of file backward_global_planner.cpp.

References cleanMarkers().

Here is the call graph for this function:

◆ configure()

void cl_nav2z::backward_global_planner::BackwardGlobalPlanner::configure ( const rclcpp_lifecycle::LifecycleNode::WeakPtr &  parent,
std::string  name,
std::shared_ptr< tf2_ros::Buffer >  tf,
std::shared_ptr< nav2_costmap_2d::Costmap2DROS >  costmap_ros 
)
virtual
Parameters
parentpointer to user's node
nameThe name of this planner
tfA pointer to a TF buffer
costmap_rosA pointer to the costmap

initialize()

Definition at line 62 of file backward_global_planner.cpp.

65{
66 this->nh_ = parent.lock();
67 name_ = name;
68 tf_ = tf;
70
71 // RCLCPP_INFO_NAMED(nh_->get_logger(), "Backwards", "BackwardGlobalPlanner initialize");
72 costmap_ros_ = costmap_ros;
73 // RCLCPP_WARN_NAMED(nh_->get_logger(), "Backwards", "Initializing global planner, costmap address: %ld",
74 // (long)costmap_ros);
75
76 planPub_ = nh_->create_publisher<nav_msgs::msg::Path>("backward_planner/global_plan", 1);
78 nh_->create_publisher<visualization_msgs::msg::MarkerArray>("backward_planner/markers", 1);
79
80 declareOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
81}
void declareOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)
Definition: common.hpp:34

References costmap_ros_, declareOrSet(), markersPub_, name_, nh_, planPub_, tf_, and transform_tolerance_.

Here is the call graph for this function:

◆ createDefaultBackwardPath()

void cl_nav2z::backward_global_planner::BackwardGlobalPlanner::createDefaultBackwardPath ( const geometry_msgs::msg::PoseStamped &  start,
const geometry_msgs::msg::PoseStamped &  goal,
std::vector< geometry_msgs::msg::PoseStamped > &  plan 
)
private

defaultBackwardPath()

Definition at line 182 of file backward_global_planner.cpp.

185{
186 auto q = start.pose.orientation;
187
188 geometry_msgs::msg::PoseStamped pose;
189 pose = start;
190
191 double dx = start.pose.position.x - goal.pose.position.x;
192 double dy = start.pose.position.y - goal.pose.position.y;
193
194 double length = sqrt(dx * dx + dy * dy);
195
196 geometry_msgs::msg::PoseStamped prevState;
198 {
199 // skip initial pure spinning and initial straight motion
200 RCLCPP_INFO(
201 nh_->get_logger(), "1 - heading to goal position pure spinning radstep: %lf",
203 double heading_direction = atan2(dy, dx);
204 double startyaw = tf2::getYaw(q);
205 double offset = angles::shortest_angular_distance(startyaw, heading_direction);
206 heading_direction = startyaw + offset;
207
208 prevState =
209 cl_nav2z::makePureSpinningSubPlan(start, heading_direction, plan, puresSpinningRadStep_);
210 RCLCPP_INFO(nh_->get_logger(), "2 - going forward keep orientation pure straight");
211
212 prevState = cl_nav2z::makePureStraightSubPlan(prevState, goal.pose.position, length, plan);
213 }
214 else
215 {
216 prevState = start;
217 }
218
219 RCLCPP_WARN_STREAM(
220 nh_->get_logger(), "[BackwardGlobalPlanner] backward global plan size: " << plan.size());
221}
geometry_msgs::msg::PoseStamped makePureSpinningSubPlan(const geometry_msgs::msg::PoseStamped &start, double dstRads, std::vector< geometry_msgs::msg::PoseStamped > &plan, double radstep=0.005)
Definition: common.cpp:37
geometry_msgs::msg::PoseStamped makePureStraightSubPlan(const geometry_msgs::msg::PoseStamped &startOrientedPose, const geometry_msgs::msg::Point &goal, double length, std::vector< geometry_msgs::msg::PoseStamped > &plan)
Definition: common.cpp:93

References cl_nav2z::makePureSpinningSubPlan(), cl_nav2z::makePureStraightSubPlan(), nh_, puresSpinningRadStep_, and skip_straight_motion_distance_.

Referenced by createPlan().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ createPlan()

nav_msgs::msg::Path cl_nav2z::backward_global_planner::BackwardGlobalPlanner::createPlan ( const geometry_msgs::msg::PoseStamped &  start,
const geometry_msgs::msg::PoseStamped &  goal 
)
virtual

Method create the plan from a starting and ending goal.

Parameters
startThe starting pose of the robot
goalThe goal pose of the robot
Returns
The sequence of poses to get from start to goal, if any

makePlan()

Definition at line 228 of file backward_global_planner.cpp.

230{
231 RCLCPP_INFO_STREAM(
232 nh_->get_logger(), "[BackwardGlobalPlanner] goal frame id: "
233 << goal.header.frame_id << " pose: " << goal.pose.position);
234 RCLCPP_INFO_STREAM(
235 nh_->get_logger(), "[BackwardGlobalPlanner] goal pose frame id: " << goal.header.frame_id);
236
237 rclcpp::Duration ttol = rclcpp::Duration::from_seconds(transform_tolerance_);
238 //---------------------------------------------------------------------
239 geometry_msgs::msg::PoseStamped transformedStart;
240 nav_2d_utils::transformPose(tf_, costmap_ros_->getGlobalFrameID(), start, transformedStart, ttol);
241 transformedStart.header.frame_id = costmap_ros_->getGlobalFrameID();
242 //---------------------------------------------------------------------
243 geometry_msgs::msg::PoseStamped transformedGoal;
244 nav_2d_utils::transformPose(tf_, costmap_ros_->getGlobalFrameID(), goal, transformedGoal, ttol);
245 transformedGoal.header.frame_id = costmap_ros_->getGlobalFrameID();
246
247 RCLCPP_INFO_STREAM(
248 nh_->get_logger(), "[BackwardGlobalPlanner] creating default backward global plan");
249 //---------------------------------------------------------------------
250 std::vector<geometry_msgs::msg::PoseStamped> plan;
251 this->createDefaultBackwardPath(transformedStart, transformedGoal, plan);
252
253 RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner] publishing markers");
254 publishGoalMarker(transformedGoal.pose, 1.0, 0, 1.0);
255
256 nav_msgs::msg::Path planMsg;
257 planMsg.poses = plan;
258 planMsg.header.stamp = this->nh_->now();
259 planMsg.header.frame_id = this->costmap_ros_->getGlobalFrameID();
260
261 //---------------------------------------------------------------------
262 // check plan rejection if obstacle is found
263 bool acceptedGlobalPlan = true;
264
265 RCLCPP_INFO_STREAM(
266 nh_->get_logger(), "[BackwardGlobalPlanner] checking backwards trajectory on costmap");
267 auto costmap2d = this->costmap_ros_->getCostmap();
268 for (auto & p : plan)
269 {
270 uint32_t mx, my;
271 costmap2d->worldToMap(p.pose.position.x, p.pose.position.y, mx, my);
272 auto cost = costmap2d->getCost(mx, my);
273
274 // static const unsigned char NO_INFORMATION = 255;
275 // static const unsigned char LETHAL_OBSTACLE = 254;
276 // static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
277 // static const unsigned char FREE_SPACE = 0;
278 if (cost >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
279 {
280 RCLCPP_WARN_STREAM(
281 nh_->get_logger(),
282 "[BackwardGlobalPlanner] backwards plan is rejected because interscts the obstacle "
283 "inscribed inflated obstacle"
284 << " at: " << p.pose.position.x << " " << p.pose.position.y);
285
286 break;
287 }
288 }
289
290 if (!acceptedGlobalPlan)
291 {
292 RCLCPP_WARN_STREAM(
293 nh_->get_logger(),
294 "[BackwardGlobalPlanner] backward plan request is not accepted, returning "
295 "empty path");
296 planMsg.poses.clear();
297 }
298
299 RCLCPP_WARN_STREAM(
300 nh_->get_logger(), "[BackwardGlobalPlanner] backward global plan publishing path. poses count: "
301 << planMsg.poses.size());
302 planPub_->publish(planMsg);
303
304 return planMsg;
305}
void publishGoalMarker(const geometry_msgs::msg::Pose &pose, double r, double g, double b)
void createDefaultBackwardPath(const geometry_msgs::msg::PoseStamped &start, const geometry_msgs::msg::PoseStamped &goal, std::vector< geometry_msgs::msg::PoseStamped > &plan)

References costmap_ros_, createDefaultBackwardPath(), nh_, planPub_, publishGoalMarker(), tf_, and transform_tolerance_.

Here is the call graph for this function:

◆ deactivate()

void cl_nav2z::backward_global_planner::BackwardGlobalPlanner::deactivate ( )
virtual

Method to deactivate planner and any threads involved in execution.

deactivate()

Definition at line 107 of file backward_global_planner.cpp.

108{
109 RCLCPP_INFO_STREAM(nh_->get_logger(), "[BackwardGlobalPlanner] deactivating planner");
110
111 // clear
112 nav_msgs::msg::Path planMsg;
113 planMsg.header.stamp = nh_->now();
114 planPub_->publish(planMsg);
115
116 planPub_->on_deactivate();
117 this->cleanMarkers();
118 markersPub_->on_deactivate();
119}

References cleanMarkers(), markersPub_, nh_, and planPub_.

Here is the call graph for this function:

◆ onForwardTrailMsg()

void cl_nav2z::backward_global_planner::BackwardGlobalPlanner::onForwardTrailMsg ( const nav_msgs::msg::Path::ConstSharedPtr &  trailMessage)
private

◆ publishGoalMarker()

void cl_nav2z::backward_global_planner::BackwardGlobalPlanner::publishGoalMarker ( const geometry_msgs::msg::Pose &  pose,
double  r,
double  g,
double  b 
)
private

publishGoalMarker()

Definition at line 126 of file backward_global_planner.cpp.

128{
129 double phi = tf2::getYaw(pose.orientation);
130 visualization_msgs::msg::Marker marker;
131 marker.header.frame_id = this->costmap_ros_->getGlobalFrameID();
132 marker.header.stamp = nh_->now();
133 marker.ns = "my_namespace2";
134 marker.id = 0;
135 marker.type = visualization_msgs::msg::Marker::ARROW;
136 marker.action = visualization_msgs::msg::Marker::ADD;
137 marker.lifetime = rclcpp::Duration(0s);
138 marker.scale.x = 0.1;
139 marker.scale.y = 0.3;
140 marker.scale.z = 0.1;
141 marker.color.a = 1.0;
142 marker.color.r = r;
143 marker.color.g = g;
144 marker.color.b = b;
145
146 geometry_msgs::msg::Point start, end;
147 start.x = pose.position.x;
148 start.y = pose.position.y;
149
150 end.x = pose.position.x + 0.5 * cos(phi);
151 end.y = pose.position.y + 0.5 * sin(phi);
152
153 marker.points.push_back(start);
154 marker.points.push_back(end);
155
156 visualization_msgs::msg::MarkerArray ma;
157 ma.markers.push_back(marker);
158
159 markersPub_->publish(ma);
160}

References costmap_ros_, markersPub_, and nh_.

Referenced by createPlan().

Here is the caller graph for this function:

Member Data Documentation

◆ costmap_ros_

std::shared_ptr<nav2_costmap_2d::Costmap2DROS> cl_nav2z::backward_global_planner::BackwardGlobalPlanner::costmap_ros_
private

Definition at line 85 of file backward_global_planner.hpp.

Referenced by cleanMarkers(), configure(), createPlan(), and publishGoalMarker().

◆ markersPub_

std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray> > cl_nav2z::backward_global_planner::BackwardGlobalPlanner::markersPub_
private

◆ name_

std::string cl_nav2z::backward_global_planner::BackwardGlobalPlanner::name_
private

Definition at line 96 of file backward_global_planner.hpp.

Referenced by configure().

◆ nh_

rclcpp_lifecycle::LifecycleNode::SharedPtr cl_nav2z::backward_global_planner::BackwardGlobalPlanner::nh_
private

◆ planPub_

std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path> > cl_nav2z::backward_global_planner::BackwardGlobalPlanner::planPub_
private

Definition at line 80 of file backward_global_planner.hpp.

Referenced by activate(), configure(), createPlan(), and deactivate().

◆ puresSpinningRadStep_

double cl_nav2z::backward_global_planner::BackwardGlobalPlanner::puresSpinningRadStep_
private

Definition at line 94 of file backward_global_planner.hpp.

Referenced by BackwardGlobalPlanner(), and createDefaultBackwardPath().

◆ skip_straight_motion_distance_

double cl_nav2z::backward_global_planner::BackwardGlobalPlanner::skip_straight_motion_distance_
private

Definition at line 92 of file backward_global_planner.hpp.

Referenced by BackwardGlobalPlanner(), and createDefaultBackwardPath().

◆ tf_

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::backward_global_planner::BackwardGlobalPlanner::tf_
private

Definition at line 98 of file backward_global_planner.hpp.

Referenced by configure(), and createPlan().

◆ transform_tolerance_

double cl_nav2z::backward_global_planner::BackwardGlobalPlanner::transform_tolerance_
private

Definition at line 100 of file backward_global_planner.hpp.

Referenced by configure(), and createPlan().


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