SMACC2
Public Member Functions | Private Member Functions | Private Attributes | List of all members
cl_nav2z::forward_local_planner::ForwardLocalPlanner Class Reference

#include <forward_local_planner.hpp>

Inheritance diagram for cl_nav2z::forward_local_planner::ForwardLocalPlanner:
Inheritance graph
Collaboration diagram for cl_nav2z::forward_local_planner::ForwardLocalPlanner:
Collaboration graph

Public Member Functions

 ForwardLocalPlanner ()
 
virtual ~ForwardLocalPlanner ()
 
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)
 
void cleanMarkers ()
 
void generateTrajectory (const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, float maxdist, float maxangle, float maxtime, float dt, std::vector< Eigen::Vector3f > &outtraj)
 
Eigen::Vector3f computeNewPositions (const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)
 

Private Attributes

nav2_util::LifecycleNode::SharedPtr nh_
 
std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmapRos_
 
std::string name_
 
rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray >::SharedPtr goalMarkerPublisher_
 
double k_rho_
 
double k_alpha_
 
double k_betta_
 
bool goalReached_
 
const double alpha_offset_ = 0
 
const double betta_offset_ = 0
 
meter carrot_distance_
 
rad carrot_angular_distance_
 
double yaw_goal_tolerance_
 
double xy_goal_tolerance_
 
double max_angular_z_speed_
 
double max_linear_x_speed_
 
double transform_tolerance_
 
int currentPoseIndex_
 
std::vector< geometry_msgs::msg::PoseStamped > plan_
 
bool waiting_
 
rclcpp::Duration waitingTimeout_
 
rclcpp::Time waitingStamp_
 
std::shared_ptr< tf2_ros::Buffer > tf_
 

Detailed Description

Definition at line 39 of file forward_local_planner.hpp.

Constructor & Destructor Documentation

◆ ForwardLocalPlanner()

cl_nav2z::forward_local_planner::ForwardLocalPlanner::ForwardLocalPlanner ( )

◆ ~ForwardLocalPlanner()

cl_nav2z::forward_local_planner::ForwardLocalPlanner::~ForwardLocalPlanner ( )
virtual

Definition at line 45 of file forward_local_planner.cpp.

45{}

Member Function Documentation

◆ activate()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::activate ( )
override

Definition at line 47 of file forward_local_planner.cpp.

48{
49 RCLCPP_DEBUG_STREAM(nh_->get_logger(), "activating controller ForwardLocalPlanner");
50 this->updateParameters();
51 this->goalMarkerPublisher_->on_activate();
52}
rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray >::SharedPtr goalMarkerPublisher_

References goalMarkerPublisher_, nh_, and updateParameters().

Here is the call graph for this function:

◆ cleanMarkers()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::cleanMarkers ( )
private

Definition at line 265 of file forward_local_planner.cpp.

266{
267 RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardLocalPlanner] cleaning markers.");
268 visualization_msgs::msg::Marker marker;
269
270 marker.header.frame_id = costmapRos_->getGlobalFrameID();
271 marker.header.stamp = nh_->now();
272 marker.ns = "my_namespace2";
273 marker.id = 0;
274 marker.action = visualization_msgs::msg::Marker::DELETEALL;
275
276 visualization_msgs::msg::MarkerArray ma;
277 ma.markers.push_back(marker);
278
279 goalMarkerPublisher_->publish(ma);
280}
std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmapRos_

References costmapRos_, goalMarkerPublisher_, and nh_.

Referenced by cleanup(), and deactivate().

Here is the caller graph for this function:

◆ cleanup()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::cleanup ( )
override

◆ computeNewPositions()

Eigen::Vector3f cl_nav2z::forward_local_planner::ForwardLocalPlanner::computeNewPositions ( const Eigen::Vector3f &  pos,
const Eigen::Vector3f &  vel,
double  dt 
)
private

Definition at line 213 of file forward_local_planner.cpp.

215{
216 Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
217 new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
218 new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
219 new_pos[2] = pos[2] + vel[2] * dt;
220 return new_pos;
221}

Referenced by generateTrajectory().

Here is the caller graph for this function:

◆ computeVelocityCommands()

geometry_msgs::msg::TwistStamped cl_nav2z::forward_local_planner::ForwardLocalPlanner::computeVelocityCommands ( const geometry_msgs::msg::PoseStamped &  currentPose,
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

computeVelocityCommands()

Definition at line 322 of file forward_local_planner.cpp.

325{
326 this->updateParameters();
327
328 if (this->plan_.size() > 0)
329 {
330 RCLCPP_INFO_STREAM(
331 nh_->get_logger(), "[ForwardLocalPlanner] Current pose frame id: "
332 << plan_.front().header.frame_id
333 << ", path pose frame id: " << currentPose.header.frame_id);
334
335 if (plan_.front().header.frame_id != currentPose.header.frame_id)
336 {
337 RCLCPP_ERROR_STREAM(nh_->get_logger(), "[ForwardLocalPlanner] Inconsistent frames");
338 }
339 }
340
341 // xy_goal_tolerance and yaw_goal_tolerance are just used for logging proposes and clamping the carrot
342 // goal distance (parameter safety)
343 if (xy_goal_tolerance_ == -1 || yaw_goal_tolerance_ == -1)
344 {
345 geometry_msgs::msg::Pose posetol;
346 geometry_msgs::msg::Twist twistol;
347 if (goal_checker->getTolerances(posetol, twistol))
348 {
349 xy_goal_tolerance_ = posetol.position.x;
350 yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation);
351 //xy_goal_tolerance_ = posetol.position.x * 0.35; // WORKAROUND DIFFERENCE WITH NAV CONTROLLER GOAL CHECKER
352 //yaw_goal_tolerance_ = tf2::getYaw(posetol.orientation) * 0.35;
353 RCLCPP_INFO_STREAM(
354 nh_->get_logger(), "[ForwardLocalPlanner] xy_goal_tolerance_: " << xy_goal_tolerance_
355 << ", yaw_goal_tolerance_: "
357 }
358 else
359 {
360 RCLCPP_INFO_STREAM(
361 nh_->get_logger(), "[ForwardLocalPlanner] could not get tolerances from goal checker");
362 }
363 }
364
365 geometry_msgs::msg::TwistStamped cmd_vel;
366 goalReached_ = false;
367 RCLCPP_DEBUG(
368 nh_->get_logger(), "[ForwardLocalPlanner] ----- COMPUTE VELOCITY COMMAND LOCAL PLANNER ---");
369
370 bool ok = false;
371 while (!ok)
372 {
373 // iterate the point from the current position and ahead until reaching a new goal point in the path
374 while (!ok && currentPoseIndex_ < (int)plan_.size())
375 {
376 auto & pose = plan_[currentPoseIndex_];
377 const geometry_msgs::msg::Point & p = pose.pose.position;
378 tf2::Quaternion q;
379 tf2::fromMsg(pose.pose.orientation, q);
380
381 // take error from the current position to the path point
382 double dx = p.x - currentPose.pose.position.x;
383 double dy = p.y - currentPose.pose.position.y;
384 double dist = sqrt(dx * dx + dy * dy);
385
386 double pangle = tf2::getYaw(q);
387 double angle = tf2::getYaw(currentPose.pose.orientation);
388 double angular_error = angles::shortest_angular_distance(pangle, angle);
389
390 if (dist >= carrot_distance_ || fabs(angular_error) > 0.1)
391 {
392 // the target pose is enough different to be defined as a target
393 ok = true;
394 RCLCPP_DEBUG(
395 nh_->get_logger(),
396 "current index: %d, carrot goal percentaje: %lf, dist: %lf, maxdist: %lf, angle_error: "
397 "%lf",
399 angular_error);
400 }
401 else
402 {
404 }
405 }
406
407 RCLCPP_DEBUG_STREAM(
408 nh_->get_logger(), "[ForwardLocalPlanner] selected carrot pose index "
409 << currentPoseIndex_ << "/" << plan_.size());
410
411 if (currentPoseIndex_ >= (int)plan_.size())
412 {
413 // even the latest point is quite similar, then take the last since it is the final goal
414 cmd_vel.twist.linear.x = 0;
415 cmd_vel.twist.angular.z = 0;
416 // RCLCPP_INFO(nh_->get_logger(), "End Local planner");
417 ok = true;
418 currentPoseIndex_ = (int)plan_.size() - 1;
419 // return true;
420 }
421 }
422
423 // RCLCPP_INFO(nh_->get_logger(), "pose control algorithm");
424
425 const geometry_msgs::msg::PoseStamped & finalgoalpose = plan_.back();
426 const geometry_msgs::msg::PoseStamped & carrot_goalpose = plan_[currentPoseIndex_];
427 const geometry_msgs::msg::Point & goalposition = carrot_goalpose.pose.position;
428
429 tf2::Quaternion carrotGoalQ;
430 tf2::fromMsg(carrot_goalpose.pose.orientation, carrotGoalQ);
431 // RCLCPP_INFO_STREAM(nh_->get_logger(), "Plan goal quaternion at "<< carrot_goalpose.pose.orientation);
432
433 // goal orientation (global frame)
434 double betta = tf2::getYaw(carrot_goalpose.pose.orientation) + betta_offset_;
435 double dx = goalposition.x - currentPose.pose.position.x;
436 double dy = goalposition.y - currentPose.pose.position.y;
437
438 // distance error to the targetpoint
439 double rho_error = sqrt(dx * dx + dy * dy);
440
441 tf2::Quaternion currentOrientation;
442 tf2::convert(currentPose.pose.orientation, currentOrientation);
443
444 // current angle
445 double theta = tf2::getYaw(currentOrientation);
446 double alpha = atan2(dy, dx);
447 alpha = alpha + alpha_offset_;
448
449 double alpha_error = angles::shortest_angular_distance(alpha, theta);
450 double betta_error = angles::shortest_angular_distance(betta, theta);
451
452 double vetta = 0; // = k_rho_ * rho_error;
453 double gamma = 0; //= k_alpha_ * alpha_error + k_betta_ * betta_error;
454
455 if (
456 rho_error >
457 xy_goal_tolerance_) // reguular control rule, be careful, rho error is with the carrot not with the
458 // final goal (this is something to improve like the backwards planner)
459 {
460 vetta = k_rho_ * rho_error;
461 gamma = k_alpha_ * alpha_error;
462 }
463 else if (fabs(betta_error) >= yaw_goal_tolerance_) // pureSpining
464 {
465 vetta = 0;
466 gamma = k_betta_ * betta_error;
467 }
468 else // goal reached
469 {
470 RCLCPP_DEBUG(nh_->get_logger(), "GOAL REACHED");
471 vetta = 0;
472 gamma = 0;
473 goalReached_ = true;
474 }
475
476 // linear speed clamp
477 if (vetta > max_linear_x_speed_)
478 {
479 vetta = max_linear_x_speed_;
480 }
481 else if (vetta < -max_linear_x_speed_)
482 {
483 vetta = -max_linear_x_speed_;
484 }
485
486 // angular speed clamp
487 if (gamma > max_angular_z_speed_)
488 {
489 gamma = max_angular_z_speed_;
490 }
491 else if (gamma < -max_angular_z_speed_)
492 {
493 gamma = -max_angular_z_speed_;
494 }
495
496 cmd_vel.twist.linear.x = vetta;
497 cmd_vel.twist.angular.z = gamma;
498
499 // clamp(cmd_vel, max_linear_x_speed_, max_angular_z_speed_);
500
501 // RCLCPP_INFO_STREAM(nh_->get_logger(), "Local planner: "<< cmd_vel);
502
503 publishGoalMarker(goalposition.x, goalposition.y, betta);
504
505 RCLCPP_DEBUG_STREAM(
506 nh_->get_logger(), "Forward local planner,"
507 << std::endl
508 << " theta: " << theta << std::endl
509 << " betta: " << betta << std::endl
510 << " err_x: " << dx << std::endl
511 << " err_y:" << dy << std::endl
512 << " rho_error:" << rho_error << std::endl
513 << " alpha_error:" << alpha_error << std::endl
514 << " betta_error:" << betta_error << std::endl
515 << " vetta:" << vetta << std::endl
516 << " gamma:" << gamma << std::endl
517 << " xy_goal_tolerance:" << xy_goal_tolerance_ << std::endl
518 << " yaw_goal_tolerance:" << yaw_goal_tolerance_ << std::endl);
519
520 // if(cmd_vel.linear.x==0 && cmd_vel.angular.z == 0 )
521 //{
522 //}
523
524 // integrate trajectory and check collision
525
526 assert(currentPose.header.frame_id == "odom" || currentPose.header.frame_id == "map");
527 auto global_pose = currentPose;
528 //->getRobotPose(global_pose);
529
530 auto * costmap2d = costmapRos_->getCostmap();
531 auto yaw = tf2::getYaw(global_pose.pose.orientation);
532
533 auto & pos = global_pose.pose.position;
534
535 Eigen::Vector3f currentpose(pos.x, pos.y, yaw);
536 Eigen::Vector3f currentvel(
537 cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z);
538
539 std::vector<Eigen::Vector3f> trajectory;
540 this->generateTrajectory(
541 currentpose, currentvel, 0.8 /*meters*/, M_PI / 8 /*rads*/, 3.0 /*seconds*/, 0.05 /*seconds*/,
542 trajectory);
543
544 // check plan rejection
545 bool aceptedplan = true;
546
547 unsigned int mx, my;
548
549 int i = 0;
550 // RCLCPP_INFO_STREAM(nh_->get_logger(), "lplanner goal: " << finalgoalpose.pose.position);
551 for (auto & p : trajectory)
552 {
553 float dx = p[0] - finalgoalpose.pose.position.x;
554 float dy = p[1] - finalgoalpose.pose.position.y;
555
556 float dst = sqrt(dx * dx + dy * dy);
557 if (dst < xy_goal_tolerance_)
558 {
559 // RCLCPP_INFO(nh_->get_logger(), "trajectory checking skipped, goal reached");
560 break;
561 }
562
563 costmap2d->worldToMap(p[0], p[1], mx, my);
564
565 // RCLCPP_INFO(nh_->get_logger(), "checking cost pt %d [%lf, %lf] cell[%d,%d] = %d", i, p[0], p[1], mx, my, cost);
566 // RCLCPP_INFO_STREAM(nh_->get_logger(), "cost: " << cost);
567
568 // static const unsigned char NO_INFORMATION = 255;
569 // static const unsigned char LETHAL_OBSTACLE = 254;
570 // static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
571 // static const unsigned char FREE_SPACE = 0;
572
573 if (costmap2d->getCost(mx, my) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
574 {
575 aceptedplan = false;
576 // RCLCPP_WARN(nh_->get_logger(), "ABORTED LOCAL PLAN BECAUSE OBSTACLE DETEDTED");
577 break;
578 }
579 i++;
580 }
581
582 bool success = false;
583 if (aceptedplan)
584 {
585 waiting_ = false;
586 success = true;
587 RCLCPP_DEBUG(nh_->get_logger(), "simulated trajectory is accepted.");
588 }
589 else
590 {
591 RCLCPP_DEBUG(nh_->get_logger(), "simulated trajectory is not accepted. Stop command.");
592
593 // stop and wait
594 cmd_vel.twist.linear.x = 0;
595 cmd_vel.twist.angular.z = 0;
596
597 if (!waiting_)
598 {
599 RCLCPP_DEBUG(nh_->get_logger(), "Start waiting obstacle disappear");
600 waiting_ = true;
601 waitingStamp_ = nh_->now();
602 }
603 else
604 {
605 auto waitingduration = nh_->now() - waitingStamp_;
606 RCLCPP_DEBUG(
607 nh_->get_logger(), "waiting obstacle disappear, elapsed: %lf seconds",
608 waitingduration.seconds());
609
610 if (waitingduration > this->waitingTimeout_)
611 {
612 RCLCPP_WARN(
613 nh_->get_logger(), "TIMEOUT waiting obstacle disappear, elapsed: %lf seconds",
614 waitingduration.seconds());
615 success = false;
616 }
617 }
618 }
619
620 if (!success)
621 {
622 RCLCPP_DEBUG(
623 nh_->get_logger(),
624 "[ForwardLocalPlanner] object detected waiting stopped until it disappears.");
625 }
626
627 cmd_vel.header.stamp = nh_->now();
628 return cmd_vel;
629}
void generateTrajectory(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, float maxdist, float maxangle, float maxtime, float dt, std::vector< Eigen::Vector3f > &outtraj)

References alpha_offset_, betta_offset_, carrot_distance_, costmapRos_, currentPoseIndex_, generateTrajectory(), goalReached_, k_alpha_, k_betta_, k_rho_, max_angular_z_speed_, max_linear_x_speed_, nh_, plan_, publishGoalMarker(), updateParameters(), waiting_, waitingStamp_, waitingTimeout_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Here is the call graph for this function:

◆ configure()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::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 69 of file forward_local_planner.cpp.

73{
74 // nh_ = rclcpp::Node::make_shared("~/ForwardLocalPlanner");
75 nh_ = node.lock();
76 costmapRos_ = costmap_ros;
77 tf_ = tf;
78 name_ = name;
79 k_rho_ = 1.0;
80 k_alpha_ = -0.4;
81 k_betta_ = -1.0; // set to zero means that orientation is not important
82 // k_betta_ = 1.0;
83 // betta_offset_=0;
84
85 goalReached_ = false;
86 carrot_distance_ = 0.4;
91
92 // rclcpp::Node::SharedPtr private_nh("~");
93
95
96 declareOrSet(nh_, name_ + ".k_rho", k_rho_);
97 declareOrSet(nh_, name_ + ".k_alpha", k_alpha_);
98 declareOrSet(nh_, name_ + ".k_betta", k_betta_);
99 declareOrSet(nh_, name_ + ".carrot_distance", carrot_distance_);
100 declareOrSet(nh_, name_ + ".yaw_goal_tolerance", yaw_goal_tolerance_);
101 declareOrSet(nh_, name_ + ".xy_goal_tolerance", xy_goal_tolerance_);
102 declareOrSet(nh_, name_ + ".max_linear_x_speed", max_linear_x_speed_);
103 declareOrSet(nh_, name_ + ".max_angular_z_speed", max_angular_z_speed_);
104 declareOrSet(nh_, name_ + ".transform_tolerance", transform_tolerance_);
105
106 RCLCPP_DEBUG(
107 nh_->get_logger(),
108 "[ForwardLocalPlanner] max linear speed: %lf, max angular speed: %lf, k_rho: %lf, "
109 "carrot_distance: "
110 "%lf, ",
112 goalMarkerPublisher_ = nh_->create_publisher<visualization_msgs::msg::MarkerArray>(
113 "forward_local_planner/carrot_goal_marker", 1);
114
115 waiting_ = false;
116 waitingTimeout_ = rclcpp::Duration(10s);
117}
void declareOrSet(rclcpp_lifecycle::LifecycleNode::SharedPtr &node, std::string param, T &value)
Definition: common.hpp:34

References carrot_distance_, costmapRos_, currentPoseIndex_, declareOrSet(), goalMarkerPublisher_, goalReached_, k_alpha_, k_betta_, k_rho_, max_angular_z_speed_, max_linear_x_speed_, name_, nh_, service_node_3::s, tf_, transform_tolerance_, waiting_, waitingTimeout_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Here is the call graph for this function:

◆ deactivate()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::deactivate ( )
override

Definition at line 54 of file forward_local_planner.cpp.

55{
56 this->cleanMarkers();
57 this->goalMarkerPublisher_->on_deactivate();
58}

References cleanMarkers(), and goalMarkerPublisher_.

Here is the call graph for this function:

◆ generateTrajectory()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::generateTrajectory ( const Eigen::Vector3f &  pos,
const Eigen::Vector3f &  vel,
float  maxdist,
float  maxangle,
float  maxtime,
float  dt,
std::vector< Eigen::Vector3f > &  outtraj 
)
private

Definition at line 148 of file forward_local_planner.cpp.

151{
152 // simulate the trajectory and check for collisions, updating costs along the way
153 bool end = false;
154 float time = 0;
155 Eigen::Vector3f currentpos = pos;
156 int i = 0;
157 while (!end)
158 {
159 // add the point to the trajectory so we can draw it later if we want
160 // traj.addPoint(pos[0], pos[1], pos[2]);
161
162 // if (continued_acceleration_) {
163 // //calculate velocities
164 // loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
165 // //RCLCPP_WARN_NAMED(nh_->get_logger(), "Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_,
166 // loop_vel[0], loop_vel[1], loop_vel[2]);
167 // }
168
169 auto loop_vel = vel;
170 // update the position of the robot using the velocities passed in
171 auto newpos = computeNewPositions(currentpos, loop_vel, dt);
172
173 auto dx = newpos[0] - currentpos[0];
174 auto dy = newpos[1] - currentpos[1];
175 float dist, angledist;
176
177 // RCLCPP_DEBUG(nh_->get_logger(), "traj point %d", i);
178 dist = sqrt(dx * dx + dy * dy);
179 if (dist > maxdist)
180 {
181 end = true;
182 // RCLCPP_DEBUG(nh_->get_logger(), "dist break: %f", dist);
183 }
184 else
185 {
186 // ouble from, double to
187 angledist = fabs(angles::shortest_angular_distance(currentpos[2], newpos[2]));
188 if (angledist > maxanglediff)
189 {
190 end = true;
191 // RCLCPP_DEBUG(nh_->get_logger(), "angle dist break: %f", angledist);
192 }
193 else
194 {
195 outtraj.push_back(newpos);
196
197 time += dt;
198 if (time > maxtime)
199 {
200 end = true;
201 // RCLCPP_DEBUG(nh_->get_logger(), "time break: %f", time);
202 }
203
204 // RCLCPP_DEBUG(nh_->get_logger(), "dist: %f, angledist: %f, time: %f", dist, angledist, time);
205 }
206 }
207
208 currentpos = newpos;
209 i++;
210 } // end for simulation steps
211}
Eigen::Vector3f computeNewPositions(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)

References computeNewPositions().

Referenced by computeVelocityCommands().

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

◆ isGoalReached()

bool cl_nav2z::forward_local_planner::ForwardLocalPlanner::isGoalReached ( )

isGoalReached()

Definition at line 644 of file forward_local_planner.cpp.

644{ return goalReached_; }

References goalReached_.

◆ publishGoalMarker()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::publishGoalMarker ( double  x,
double  y,
double  phi 
)
private

publishGoalMarker()

Definition at line 228 of file forward_local_planner.cpp.

229{
230 visualization_msgs::msg::Marker marker;
231
232 marker.header.frame_id = costmapRos_->getGlobalFrameID();
233 marker.header.stamp = nh_->now();
234 marker.ns = "my_namespace2";
235 marker.id = 0;
236 marker.type = visualization_msgs::msg::Marker::ARROW;
237 marker.action = visualization_msgs::msg::Marker::ADD;
238 marker.pose.orientation.w = 1;
239 marker.lifetime = rclcpp::Duration(1.0s);
240
241 marker.scale.x = 0.1;
242 marker.scale.y = 0.3;
243 marker.scale.z = 0.1;
244 marker.color.a = 1.0;
245 marker.color.r = 0;
246 marker.color.g = 0;
247 marker.color.b = 1.0;
248
249 geometry_msgs::msg::Point start, end;
250 start.x = x;
251 start.y = y;
252
253 end.x = x + 0.5 * cos(phi);
254 end.y = y + 0.5 * sin(phi);
255
256 marker.points.push_back(start);
257 marker.points.push_back(end);
258
259 visualization_msgs::msg::MarkerArray ma;
260 ma.markers.push_back(marker);
261
262 goalMarkerPublisher_->publish(ma);
263}

References costmapRos_, goalMarkerPublisher_, nh_, and service_node_3::s.

Referenced by computeVelocityCommands().

Here is the caller graph for this function:

◆ setPlan()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::setPlan ( const nav_msgs::msg::Path &  plan)
override

nav2_core setPlan - Sets the global plan

Parameters
pathThe global plan

setPlan()

Definition at line 651 of file forward_local_planner.cpp.

652{
653 nav_msgs::msg::Path transformedPlan;
654
655 rclcpp::Duration ttol = rclcpp::Duration::from_seconds(transform_tolerance_);
656 // transform global plan
657 for (auto & p : plan.poses)
658 {
659 geometry_msgs::msg::PoseStamped transformedPose;
660 nav_2d_utils::transformPose(tf_, costmapRos_->getGlobalFrameID(), p, transformedPose, ttol);
661 transformedPose.header.frame_id = costmapRos_->getGlobalFrameID();
662 transformedPlan.poses.push_back(transformedPose);
663 }
664
665 plan_ = transformedPlan.poses;
666 goalReached_ = false;
667}

References costmapRos_, goalReached_, plan_, tf_, and transform_tolerance_.

◆ setSpeedLimit()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::setSpeedLimit ( const double &  speed_limit,
const bool percentage 
)
overridevirtual

Definition at line 631 of file forward_local_planner.cpp.

632{
633 RCLCPP_WARN_STREAM(
634 nh_->get_logger(),
635 "ForwardLocalPlanner::setSpeedLimit invoked. Ignored, funcionality not "
636 "implemented.");
637}

References nh_.

◆ updateParameters()

void cl_nav2z::forward_local_planner::ForwardLocalPlanner::updateParameters ( )
private

Definition at line 119 of file forward_local_planner.cpp.

120{
121 nh_->get_parameter(name_ + ".k_rho", k_rho_);
122 nh_->get_parameter(name_ + ".k_alpha", k_alpha_);
123 nh_->get_parameter(name_ + ".k_betta", k_betta_);
124 nh_->get_parameter(name_ + ".carrot_distance", carrot_distance_);
125 nh_->get_parameter(name_ + ".yaw_goal_tolerance", yaw_goal_tolerance_);
126 nh_->get_parameter(name_ + ".xy_goal_tolerance", xy_goal_tolerance_);
127 nh_->get_parameter(name_ + ".max_linear_x_speed", max_linear_x_speed_);
128 nh_->get_parameter(name_ + ".max_angular_z_speed", max_angular_z_speed_);
129 nh_->get_parameter(name_ + ".transform_tolerance", transform_tolerance_);
130
131 RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardLocalPlanner.k_rho: " << k_rho_);
132 RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardLocalPlanner.k_alpha: " << k_alpha_);
133 RCLCPP_INFO_STREAM(nh_->get_logger(), "[ForwardLocalPlanner.k_betta: " << k_betta_);
134 RCLCPP_INFO_STREAM(
135 nh_->get_logger(), "[ForwardLocalPlanner.carrot_distance: " << carrot_distance_);
136 RCLCPP_INFO_STREAM(
137 nh_->get_logger(), "[ForwardLocalPlanner.yaw_goal_tolerance:" << yaw_goal_tolerance_);
138 RCLCPP_INFO_STREAM(
139 nh_->get_logger(), "[ForwardLocalPlanner.xy_goal_tolerance: " << xy_goal_tolerance_);
140 RCLCPP_INFO_STREAM(
141 nh_->get_logger(), "[ForwardLocalPlanner.max_linear_x_speed:" << max_linear_x_speed_);
142 RCLCPP_INFO_STREAM(
143 nh_->get_logger(), "[ForwardLocalPlanner.max_angular_z_speed:" << max_angular_z_speed_);
144 RCLCPP_INFO_STREAM(
145 nh_->get_logger(), "[ForwardLocalPlanner.transform_tolerance:" << transform_tolerance_);
146}

References carrot_distance_, k_alpha_, k_betta_, k_rho_, max_angular_z_speed_, max_linear_x_speed_, name_, nh_, transform_tolerance_, xy_goal_tolerance_, and yaw_goal_tolerance_.

Referenced by activate(), and computeVelocityCommands().

Here is the caller graph for this function:

Member Data Documentation

◆ alpha_offset_

const double cl_nav2z::forward_local_planner::ForwardLocalPlanner::alpha_offset_ = 0
private

Definition at line 106 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands().

◆ betta_offset_

const double cl_nav2z::forward_local_planner::ForwardLocalPlanner::betta_offset_ = 0
private

Definition at line 107 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands().

◆ carrot_angular_distance_

rad cl_nav2z::forward_local_planner::ForwardLocalPlanner::carrot_angular_distance_
private

Definition at line 110 of file forward_local_planner.hpp.

◆ carrot_distance_

meter cl_nav2z::forward_local_planner::ForwardLocalPlanner::carrot_distance_
private

Definition at line 109 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

◆ costmapRos_

std::shared_ptr<nav2_costmap_2d::Costmap2DROS> cl_nav2z::forward_local_planner::ForwardLocalPlanner::costmapRos_
private

◆ currentPoseIndex_

int cl_nav2z::forward_local_planner::ForwardLocalPlanner::currentPoseIndex_
private

Definition at line 120 of file forward_local_planner.hpp.

Referenced by cleanup(), computeVelocityCommands(), and configure().

◆ goalMarkerPublisher_

rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr cl_nav2z::forward_local_planner::ForwardLocalPlanner::goalMarkerPublisher_
private

◆ goalReached_

bool cl_nav2z::forward_local_planner::ForwardLocalPlanner::goalReached_
private

◆ k_alpha_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::k_alpha_
private

Definition at line 102 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

◆ k_betta_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::k_betta_
private

Definition at line 103 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

◆ k_rho_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::k_rho_
private

Definition at line 101 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

◆ max_angular_z_speed_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::max_angular_z_speed_
private

Definition at line 115 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

◆ max_linear_x_speed_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::max_linear_x_speed_
private

Definition at line 116 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), configure(), and updateParameters().

◆ name_

std::string cl_nav2z::forward_local_planner::ForwardLocalPlanner::name_
private

Definition at line 96 of file forward_local_planner.hpp.

Referenced by configure(), and updateParameters().

◆ nh_

nav2_util::LifecycleNode::SharedPtr cl_nav2z::forward_local_planner::ForwardLocalPlanner::nh_
private

◆ plan_

std::vector<geometry_msgs::msg::PoseStamped> cl_nav2z::forward_local_planner::ForwardLocalPlanner::plan_
private

Definition at line 122 of file forward_local_planner.hpp.

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

◆ tf_

std::shared_ptr<tf2_ros::Buffer> cl_nav2z::forward_local_planner::ForwardLocalPlanner::tf_
private

Definition at line 127 of file forward_local_planner.hpp.

Referenced by configure(), and setPlan().

◆ transform_tolerance_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::transform_tolerance_
private

Definition at line 117 of file forward_local_planner.hpp.

Referenced by configure(), setPlan(), and updateParameters().

◆ waiting_

bool cl_nav2z::forward_local_planner::ForwardLocalPlanner::waiting_
private

Definition at line 124 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), and configure().

◆ waitingStamp_

rclcpp::Time cl_nav2z::forward_local_planner::ForwardLocalPlanner::waitingStamp_
private

Definition at line 126 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands().

◆ waitingTimeout_

rclcpp::Duration cl_nav2z::forward_local_planner::ForwardLocalPlanner::waitingTimeout_
private

Definition at line 125 of file forward_local_planner.hpp.

Referenced by computeVelocityCommands(), and configure().

◆ xy_goal_tolerance_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::xy_goal_tolerance_
private

◆ yaw_goal_tolerance_

double cl_nav2z::forward_local_planner::ForwardLocalPlanner::yaw_goal_tolerance_
private

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