Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[ros2-master] Fix build for last nav2 main branch #274

Merged
merged 4 commits into from
Mar 9, 2021
Merged

[ros2-master] Fix build for last nav2 main branch #274

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
1de5588
build with current nav2 master
Feb 9, 2021
4dd2407
setSpeedLimit override to conform to API, needs to be implemented
Feb 9, 2021
9ef8972
build with new setSpeedLimit interface (todo: implementation)
Mar 5, 2021
64bff7f
setSpeedLimit implementation
Mar 9, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions teb_local_planner/include/teb_local_planner/teb_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,10 @@ class TebConfig
//! Robot related parameters
struct Robot
{
double base_max_vel_x; //!< Maximum translational velocity of the robot before speed limit is applied
double base_max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards before speed limit is applied
double base_max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) before speed limit is applied
double base_max_vel_theta; //!< Maximum angular velocity of the robot before speed limit is applied
double max_vel_x; //!< Maximum translational velocity of the robot
double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards
double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
Expand Down Expand Up @@ -258,6 +262,10 @@ class TebConfig
robot.max_vel_x_backwards = 0.2;
robot.max_vel_y = 0.0;
robot.max_vel_theta = 0.3;
robot.base_max_vel_x = robot.max_vel_x;
robot.base_max_vel_x_backwards = robot.base_max_vel_x_backwards;
robot.base_max_vel_y = robot.base_max_vel_y;
robot.base_max_vel_theta = robot.base_max_vel_theta;
robot.acc_lim_x = 0.5;
robot.acc_lim_y = 0.5;
robot.acc_lim_theta = 0.5;
Expand Down
16 changes: 13 additions & 3 deletions teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,7 @@

// costmap
#include <costmap_converter/costmap_converter_interface.h>
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"

#include <nav2_util/lifecycle_node.hpp>
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
Expand Down Expand Up @@ -106,7 +107,7 @@ class TebLocalPlannerROS : public nav2_core::Controller
* @param costmap_ros Cost map representing occupied and free space
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & node,
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;
Expand Down Expand Up @@ -348,10 +349,19 @@ class TebLocalPlannerROS : public nav2_core::Controller

void configureBackupModes(std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan, int& goal_idx);

/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value (in m/s)
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
void setSpeedLimit(const double & speed_limit, const bool & percentage);

private:
// Definition of member variables
nav2_util::LifecycleNode::WeakPtr nh_;
rclcpp::Logger logger_;
rclcpp_lifecycle::LifecycleNode::WeakPtr nh_;
rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")};
rclcpp::Clock::SharedPtr clock_;
rclcpp::Node::SharedPtr intra_proc_node_;
// external objects (store weak pointers)
Expand Down
44 changes: 40 additions & 4 deletions teb_local_planner/src/teb_local_planner_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ namespace teb_local_planner


TebLocalPlannerROS::TebLocalPlannerROS()
: nh_(nullptr), costmap_ros_(nullptr), tf_(nullptr), cfg_(new TebConfig()), costmap_model_(nullptr), intra_proc_node_(nullptr),
: costmap_ros_(nullptr), tf_(nullptr), cfg_(new TebConfig()), costmap_model_(nullptr), intra_proc_node_(nullptr),
costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"),
custom_via_points_active_(false), no_infeasible_plans_(0),
last_preferred_rotdir_(RotType::none), initialized_(false)
Expand Down Expand Up @@ -201,11 +201,11 @@ void TebLocalPlannerROS::initialize(nav2_util::LifecycleNode::SharedPtr node)
}

void TebLocalPlannerROS::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & node,
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) {
nh_ = node;
nh_ = parent;

auto node = nh_.lock();
logger_ = node->get_logger();
Expand Down Expand Up @@ -1008,7 +1008,43 @@ void TebLocalPlannerROS::configureBackupModes(std::vector<geometry_msgs::msg::Po

}



void TebLocalPlannerROS::setSpeedLimit(
const double & speed_limit, const bool & percentage)
{
if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
// Restore default value
cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x;
cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards;
cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y;
cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta;
Comment on lines +1018 to +1020
Copy link

@tonynajjar tonynajjar Jul 11, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Isn't this supposed to be:

cfg_->robot.max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * speed_limit / 100.0;
cfg_->robot.max_vel_y = cfg_->robot.base_max_vel_y * speed_limit / 100.0;
cfg_->robot.max_vel_theta = cfg_->robot.base_max_vel_theta * speed_limit / 100.0;

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These lines are inside :
if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
so I believe this is the intended behavior

Copy link

@tonynajjar tonynajjar Jul 11, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry I copied the wrong snippet, I meant that we should be setting max_vel_* and not base_max_vel_*

See our attempted fix https://github.com/logivations/teb_local_planner/pull/33/files

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Created PR #372

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nice catch

} else {
if (percentage) {
// Speed limit is expressed in % from maximum speed of robot
cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * speed_limit / 100.0;
cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * speed_limit / 100.0;
cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * speed_limit / 100.0;
cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * speed_limit / 100.0;
} else {
// Speed limit is expressed in absolute value
double max_speed_xy = std::max(
std::max(cfg_->robot.base_max_vel_x,cfg_->robot.base_max_vel_x_backwards),cfg_->robot.base_max_vel_y);
if (speed_limit < max_speed_xy) {
// Handling components and angular velocity changes:
// Max velocities are being changed in the same proportion
// as absolute linear speed changed in order to preserve
// robot moving trajectories to be the same after speed change.
// G. Doisy: not sure if that's applicable to base_max_vel_x_backwards.
const double ratio = speed_limit / max_speed_xy;
cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * ratio;
cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * ratio;
cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * ratio;
cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * ratio;
}
}
}
}

void TebLocalPlannerROS::customObstacleCB(const costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr obst_msg)
{
std::lock_guard<std::mutex> l(custom_obst_mutex_);
Expand Down