add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,226 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_plugins/kinematic_parameters.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
|
||||
|
||||
using nav2_util::declare_parameter_if_not_declared;
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using std::placeholders::_1;
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
|
||||
KinematicsHandler::KinematicsHandler()
|
||||
{
|
||||
kinematics_.store(new KinematicParameters);
|
||||
}
|
||||
|
||||
KinematicsHandler::~KinematicsHandler()
|
||||
{
|
||||
delete kinematics_.load();
|
||||
}
|
||||
|
||||
void KinematicsHandler::initialize(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh,
|
||||
const std::string & plugin_name)
|
||||
{
|
||||
plugin_name_ = plugin_name;
|
||||
|
||||
declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_x", rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(nh, plugin_name + ".min_vel_y", rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(nh, plugin_name + ".max_vel_x", rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(nh, plugin_name + ".max_vel_y", rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(
|
||||
nh, plugin_name + ".max_vel_theta",
|
||||
rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(
|
||||
nh, plugin_name + ".min_speed_xy",
|
||||
rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(
|
||||
nh, plugin_name + ".max_speed_xy",
|
||||
rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(
|
||||
nh, plugin_name + ".min_speed_theta",
|
||||
rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(nh, plugin_name + ".acc_lim_x", rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(nh, plugin_name + ".acc_lim_y", rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(
|
||||
nh, plugin_name + ".acc_lim_theta",
|
||||
rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(nh, plugin_name + ".decel_lim_x", rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(nh, plugin_name + ".decel_lim_y", rclcpp::ParameterValue(0.0));
|
||||
declare_parameter_if_not_declared(
|
||||
nh, plugin_name + ".decel_lim_theta",
|
||||
rclcpp::ParameterValue(0.0));
|
||||
|
||||
KinematicParameters kinematics;
|
||||
|
||||
nh->get_parameter(plugin_name + ".min_vel_x", kinematics.min_vel_x_);
|
||||
nh->get_parameter(plugin_name + ".min_vel_y", kinematics.min_vel_y_);
|
||||
nh->get_parameter(plugin_name + ".max_vel_x", kinematics.max_vel_x_);
|
||||
nh->get_parameter(plugin_name + ".max_vel_y", kinematics.max_vel_y_);
|
||||
nh->get_parameter(plugin_name + ".max_vel_theta", kinematics.max_vel_theta_);
|
||||
nh->get_parameter(plugin_name + ".min_speed_xy", kinematics.min_speed_xy_);
|
||||
nh->get_parameter(plugin_name + ".max_speed_xy", kinematics.max_speed_xy_);
|
||||
nh->get_parameter(plugin_name + ".min_speed_theta", kinematics.min_speed_theta_);
|
||||
nh->get_parameter(plugin_name + ".acc_lim_x", kinematics.acc_lim_x_);
|
||||
nh->get_parameter(plugin_name + ".acc_lim_y", kinematics.acc_lim_y_);
|
||||
nh->get_parameter(plugin_name + ".acc_lim_theta", kinematics.acc_lim_theta_);
|
||||
nh->get_parameter(plugin_name + ".decel_lim_x", kinematics.decel_lim_x_);
|
||||
nh->get_parameter(plugin_name + ".decel_lim_y", kinematics.decel_lim_y_);
|
||||
nh->get_parameter(plugin_name + ".decel_lim_theta", kinematics.decel_lim_theta_);
|
||||
|
||||
kinematics.base_max_vel_x_ = kinematics.max_vel_x_;
|
||||
kinematics.base_max_vel_y_ = kinematics.max_vel_y_;
|
||||
kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_;
|
||||
kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
|
||||
|
||||
// Add callback for dynamic parameters
|
||||
dyn_params_handler_ = nh->add_on_set_parameters_callback(
|
||||
std::bind(&KinematicsHandler::dynamicParametersCallback, this, _1));
|
||||
|
||||
kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
|
||||
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
|
||||
|
||||
update_kinematics(kinematics);
|
||||
}
|
||||
|
||||
void KinematicsHandler::setSpeedLimit(
|
||||
const double & speed_limit, const bool & percentage)
|
||||
{
|
||||
KinematicParameters kinematics(*kinematics_.load());
|
||||
|
||||
if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
|
||||
// Restore default value
|
||||
kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_;
|
||||
kinematics.max_vel_x_ = kinematics.base_max_vel_x_;
|
||||
kinematics.max_vel_y_ = kinematics.base_max_vel_y_;
|
||||
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_;
|
||||
} else {
|
||||
if (percentage) {
|
||||
// Speed limit is expressed in % from maximum speed of robot
|
||||
kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0;
|
||||
kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0;
|
||||
kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0;
|
||||
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0;
|
||||
} else {
|
||||
// Speed limit is expressed in absolute value
|
||||
if (speed_limit < kinematics.base_max_speed_xy_) {
|
||||
kinematics.max_speed_xy_ = speed_limit;
|
||||
// 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.
|
||||
const double ratio = speed_limit / kinematics.base_max_speed_xy_;
|
||||
kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * ratio;
|
||||
kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * ratio;
|
||||
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * ratio;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Do not forget to update max_speed_xy_sq_ as well
|
||||
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
|
||||
|
||||
update_kinematics(kinematics);
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
KinematicsHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
KinematicParameters kinematics(*kinematics_.load());
|
||||
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == plugin_name_ + ".min_vel_x") {
|
||||
kinematics.min_vel_x_ = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".min_vel_y") {
|
||||
kinematics.min_vel_y_ = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".max_vel_x") {
|
||||
kinematics.max_vel_x_ = parameter.as_double();
|
||||
kinematics.base_max_vel_x_ = kinematics.max_vel_x_;
|
||||
} else if (name == plugin_name_ + ".max_vel_y") {
|
||||
kinematics.max_vel_y_ = parameter.as_double();
|
||||
kinematics.base_max_vel_y_ = kinematics.max_vel_y_;
|
||||
} else if (name == plugin_name_ + ".max_vel_theta") {
|
||||
kinematics.max_vel_theta_ = parameter.as_double();
|
||||
kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
|
||||
} else if (name == plugin_name_ + ".min_speed_xy") {
|
||||
kinematics.min_speed_xy_ = parameter.as_double();
|
||||
kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
|
||||
} else if (name == plugin_name_ + ".max_speed_xy") {
|
||||
kinematics.max_speed_xy_ = parameter.as_double();
|
||||
kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_;
|
||||
} else if (name == plugin_name_ + ".min_speed_theta") {
|
||||
kinematics.min_speed_theta_ = parameter.as_double();
|
||||
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
|
||||
} else if (name == plugin_name_ + ".acc_lim_x") {
|
||||
kinematics.acc_lim_x_ = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".acc_lim_y") {
|
||||
kinematics.acc_lim_y_ = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".acc_lim_theta") {
|
||||
kinematics.acc_lim_theta_ = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".decel_lim_x") {
|
||||
kinematics.decel_lim_x_ = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".decel_lim_y") {
|
||||
kinematics.decel_lim_y_ = parameter.as_double();
|
||||
} else if (name == plugin_name_ + ".decel_lim_theta") {
|
||||
kinematics.decel_lim_theta_ = parameter.as_double();
|
||||
}
|
||||
}
|
||||
}
|
||||
update_kinematics(kinematics);
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
void KinematicsHandler::update_kinematics(KinematicParameters kinematics)
|
||||
{
|
||||
delete kinematics_.load();
|
||||
kinematics_.store(new KinematicParameters(kinematics));
|
||||
}
|
||||
|
||||
} // namespace dwb_plugins
|
||||
@@ -0,0 +1,98 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_plugins/limited_accel_generator.hpp"
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
|
||||
void LimitedAccelGenerator::initialize(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh,
|
||||
const std::string & plugin_name)
|
||||
{
|
||||
plugin_name_ = plugin_name;
|
||||
StandardTrajectoryGenerator::initialize(nh, plugin_name_);
|
||||
|
||||
try {
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh, plugin_name + ".sim_period", rclcpp::PARAMETER_DOUBLE);
|
||||
if (!nh->get_parameter(plugin_name + ".sim_period", acceleration_time_)) {
|
||||
// This actually should never appear, since declare_parameter_if_not_declared()
|
||||
// completed w/o exceptions guarantee that static parameter will be initialized
|
||||
// with some value. However for reliability we should also process the case
|
||||
// when get_parameter() will return a failure for some other reasons.
|
||||
throw std::runtime_error("Failed to get 'sim_period' value");
|
||||
}
|
||||
} catch (std::exception &) {
|
||||
RCLCPP_WARN(
|
||||
rclcpp::get_logger("LimitedAccelGenerator"),
|
||||
"'sim_period' parameter is not set for %s", plugin_name.c_str());
|
||||
double controller_frequency = nav_2d_utils::searchAndGetParam(
|
||||
nh, "controller_frequency", 20.0);
|
||||
if (controller_frequency > 0) {
|
||||
acceleration_time_ = 1.0 / controller_frequency;
|
||||
} else {
|
||||
RCLCPP_WARN(
|
||||
rclcpp::get_logger("LimitedAccelGenerator"),
|
||||
"A controller_frequency less than or equal to 0 has been set. "
|
||||
"Ignoring the parameter, assuming a rate of 20Hz");
|
||||
acceleration_time_ = 0.05;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void LimitedAccelGenerator::startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity)
|
||||
{
|
||||
// Limit our search space to just those within the limited acceleration_time
|
||||
velocity_iterator_->startNewIteration(current_velocity, acceleration_time_);
|
||||
}
|
||||
|
||||
nav_2d_msgs::msg::Twist2D LimitedAccelGenerator::computeNewVelocity(
|
||||
const nav_2d_msgs::msg::Twist2D & cmd_vel,
|
||||
const nav_2d_msgs::msg::Twist2D & /*start_vel*/,
|
||||
const double /*dt*/)
|
||||
{
|
||||
return cmd_vel;
|
||||
}
|
||||
|
||||
} // namespace dwb_plugins
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_plugins::LimitedAccelGenerator, dwb_core::TrajectoryGenerator)
|
||||
@@ -0,0 +1,228 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_plugins/standard_traj_generator.hpp"
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
#include "dwb_plugins/xy_theta_iterator.hpp"
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
|
||||
void StandardTrajectoryGenerator::initialize(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh,
|
||||
const std::string & plugin_name)
|
||||
{
|
||||
plugin_name_ = plugin_name;
|
||||
kinematics_handler_ = std::make_shared<KinematicsHandler>();
|
||||
kinematics_handler_->initialize(nh, plugin_name_);
|
||||
initializeIterator(nh);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".sim_time", rclcpp::ParameterValue(1.7));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".discretize_by_time", rclcpp::ParameterValue(false));
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".time_granularity", rclcpp::ParameterValue(0.5));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".linear_granularity", rclcpp::ParameterValue(0.5));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".angular_granularity", rclcpp::ParameterValue(0.025));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".include_last_point", rclcpp::ParameterValue(true));
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".limit_vel_cmd_in_traj", rclcpp::ParameterValue(false));
|
||||
|
||||
/*
|
||||
* If discretize_by_time, then sim_granularity represents the amount of time that should be between
|
||||
* two successive points on the trajectory.
|
||||
*
|
||||
* If discretize_by_time is false, then sim_granularity is the maximum amount of distance between
|
||||
* two successive points on the trajectory, and angular_sim_granularity is the maximum amount of
|
||||
* angular distance between two successive points.
|
||||
*/
|
||||
nh->get_parameter(plugin_name + ".sim_time", sim_time_);
|
||||
nh->get_parameter(plugin_name + ".discretize_by_time", discretize_by_time_);
|
||||
nh->get_parameter(plugin_name + ".time_granularity", time_granularity_);
|
||||
nh->get_parameter(plugin_name + ".linear_granularity", linear_granularity_);
|
||||
nh->get_parameter(plugin_name + ".angular_granularity", angular_granularity_);
|
||||
nh->get_parameter(plugin_name + ".include_last_point", include_last_point_);
|
||||
nh->get_parameter(plugin_name + ".limit_vel_cmd_in_traj", limit_vel_cmd_in_traj_);
|
||||
}
|
||||
|
||||
void StandardTrajectoryGenerator::initializeIterator(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh)
|
||||
{
|
||||
velocity_iterator_ = std::make_shared<XYThetaIterator>();
|
||||
velocity_iterator_->initialize(nh, kinematics_handler_, plugin_name_);
|
||||
}
|
||||
|
||||
void StandardTrajectoryGenerator::startNewIteration(
|
||||
const nav_2d_msgs::msg::Twist2D & current_velocity)
|
||||
{
|
||||
velocity_iterator_->startNewIteration(current_velocity, sim_time_);
|
||||
}
|
||||
|
||||
bool StandardTrajectoryGenerator::hasMoreTwists()
|
||||
{
|
||||
return velocity_iterator_->hasMoreTwists();
|
||||
}
|
||||
|
||||
nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::nextTwist()
|
||||
{
|
||||
return velocity_iterator_->nextTwist();
|
||||
}
|
||||
|
||||
std::vector<double> StandardTrajectoryGenerator::getTimeSteps(
|
||||
const nav_2d_msgs::msg::Twist2D & cmd_vel)
|
||||
{
|
||||
std::vector<double> steps;
|
||||
if (discretize_by_time_) {
|
||||
steps.resize(ceil(sim_time_ / time_granularity_));
|
||||
} else { // discretize by distance
|
||||
double vmag = hypot(cmd_vel.x, cmd_vel.y);
|
||||
|
||||
// the distance the robot would travel in sim_time if it did not change velocity
|
||||
double projected_linear_distance = vmag * sim_time_;
|
||||
|
||||
// the angle the robot would rotate in sim_time
|
||||
double projected_angular_distance = fabs(cmd_vel.theta) * sim_time_;
|
||||
|
||||
// Pick the maximum of the two
|
||||
int num_steps = ceil(
|
||||
std::max(
|
||||
projected_linear_distance / linear_granularity_,
|
||||
projected_angular_distance / angular_granularity_));
|
||||
steps.resize(num_steps);
|
||||
}
|
||||
if (steps.size() == 0) {
|
||||
steps.resize(1);
|
||||
}
|
||||
std::fill(steps.begin(), steps.end(), sim_time_ / steps.size());
|
||||
return steps;
|
||||
}
|
||||
|
||||
dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(
|
||||
const geometry_msgs::msg::Pose2D & start_pose,
|
||||
const nav_2d_msgs::msg::Twist2D & start_vel,
|
||||
const nav_2d_msgs::msg::Twist2D & cmd_vel)
|
||||
{
|
||||
dwb_msgs::msg::Trajectory2D traj;
|
||||
traj.velocity = cmd_vel;
|
||||
// simulate the trajectory
|
||||
geometry_msgs::msg::Pose2D pose = start_pose;
|
||||
nav_2d_msgs::msg::Twist2D vel = start_vel;
|
||||
double running_time = 0.0;
|
||||
std::vector<double> steps = getTimeSteps(cmd_vel);
|
||||
traj.poses.push_back(start_pose);
|
||||
bool first_vel = false;
|
||||
for (double dt : steps) {
|
||||
// calculate velocities
|
||||
vel = computeNewVelocity(cmd_vel, vel, dt);
|
||||
if (!first_vel && limit_vel_cmd_in_traj_) {
|
||||
traj.velocity = vel;
|
||||
first_vel = true;
|
||||
}
|
||||
|
||||
// update the position of the robot using the velocities passed in
|
||||
pose = computeNewPosition(pose, vel, dt);
|
||||
|
||||
traj.poses.push_back(pose);
|
||||
traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
|
||||
running_time += dt;
|
||||
} // end for simulation steps
|
||||
|
||||
if (include_last_point_) {
|
||||
traj.poses.push_back(pose);
|
||||
traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
|
||||
}
|
||||
|
||||
return traj;
|
||||
}
|
||||
|
||||
/**
|
||||
* change vel using acceleration limits to converge towards sample_target-vel
|
||||
*/
|
||||
nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::computeNewVelocity(
|
||||
const nav_2d_msgs::msg::Twist2D & cmd_vel,
|
||||
const nav_2d_msgs::msg::Twist2D & start_vel, const double dt)
|
||||
{
|
||||
KinematicParameters kinematics = kinematics_handler_->getKinematics();
|
||||
nav_2d_msgs::msg::Twist2D new_vel;
|
||||
new_vel.x = projectVelocity(
|
||||
start_vel.x, kinematics.getAccX(),
|
||||
kinematics.getDecelX(), dt, cmd_vel.x);
|
||||
new_vel.y = projectVelocity(
|
||||
start_vel.y, kinematics.getAccY(),
|
||||
kinematics.getDecelY(), dt, cmd_vel.y);
|
||||
new_vel.theta = projectVelocity(
|
||||
start_vel.theta,
|
||||
kinematics.getAccTheta(), kinematics.getDecelTheta(),
|
||||
dt, cmd_vel.theta);
|
||||
return new_vel;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Pose2D StandardTrajectoryGenerator::computeNewPosition(
|
||||
const geometry_msgs::msg::Pose2D start_pose,
|
||||
const nav_2d_msgs::msg::Twist2D & vel, const double dt)
|
||||
{
|
||||
geometry_msgs::msg::Pose2D new_pose;
|
||||
new_pose.x = start_pose.x +
|
||||
(vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt;
|
||||
new_pose.y = start_pose.y +
|
||||
(vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
|
||||
new_pose.theta = start_pose.theta + vel.theta * dt;
|
||||
return new_pose;
|
||||
}
|
||||
|
||||
} // namespace dwb_plugins
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
dwb_plugins::StandardTrajectoryGenerator,
|
||||
dwb_core::TrajectoryGenerator)
|
||||
@@ -0,0 +1,154 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "dwb_plugins/xy_theta_iterator.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav_2d_utils/parameters.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
#define EPSILON 1E-5
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
void XYThetaIterator::initialize(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh,
|
||||
KinematicsHandler::Ptr kinematics,
|
||||
const std::string & plugin_name)
|
||||
{
|
||||
kinematics_handler_ = kinematics;
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".vx_samples", rclcpp::ParameterValue(20));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".vy_samples", rclcpp::ParameterValue(5));
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh,
|
||||
plugin_name + ".vtheta_samples", rclcpp::ParameterValue(20));
|
||||
|
||||
nh->get_parameter(plugin_name + ".vx_samples", vx_samples_);
|
||||
nh->get_parameter(plugin_name + ".vy_samples", vy_samples_);
|
||||
nh->get_parameter(plugin_name + ".vtheta_samples", vtheta_samples_);
|
||||
}
|
||||
|
||||
void XYThetaIterator::startNewIteration(
|
||||
const nav_2d_msgs::msg::Twist2D & current_velocity,
|
||||
double dt)
|
||||
{
|
||||
KinematicParameters kinematics = kinematics_handler_->getKinematics();
|
||||
x_it_ = std::make_shared<OneDVelocityIterator>(
|
||||
current_velocity.x,
|
||||
kinematics.getMinX(), kinematics.getMaxX(),
|
||||
kinematics.getAccX(), kinematics.getDecelX(),
|
||||
dt, vx_samples_);
|
||||
y_it_ = std::make_shared<OneDVelocityIterator>(
|
||||
current_velocity.y,
|
||||
kinematics.getMinY(), kinematics.getMaxY(),
|
||||
kinematics.getAccY(), kinematics.getDecelY(),
|
||||
dt, vy_samples_);
|
||||
th_it_ = std::make_shared<OneDVelocityIterator>(
|
||||
current_velocity.theta,
|
||||
kinematics.getMinTheta(), kinematics.getMaxTheta(),
|
||||
kinematics.getAccTheta(), kinematics.getDecelTheta(),
|
||||
dt, vtheta_samples_);
|
||||
if (!isValidVelocity()) {
|
||||
iterateToValidVelocity();
|
||||
}
|
||||
}
|
||||
|
||||
bool XYThetaIterator::isValidSpeed(double x, double y, double theta)
|
||||
{
|
||||
KinematicParameters kinematics = kinematics_handler_->getKinematics();
|
||||
double vmag_sq = x * x + y * y;
|
||||
if (kinematics.getMaxSpeedXY() >= 0.0 && vmag_sq > kinematics.getMaxSpeedXY_SQ() + EPSILON) {
|
||||
return false;
|
||||
}
|
||||
if (kinematics.getMinSpeedXY() >= 0.0 && vmag_sq + EPSILON < kinematics.getMinSpeedXY_SQ() &&
|
||||
kinematics.getMinSpeedTheta() >= 0.0 && fabs(theta) + EPSILON < kinematics.getMinSpeedTheta())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (vmag_sq == 0.0 && th_it_->getVelocity() == 0.0) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool XYThetaIterator::isValidVelocity()
|
||||
{
|
||||
return isValidSpeed(
|
||||
x_it_->getVelocity(), y_it_->getVelocity(),
|
||||
th_it_->getVelocity());
|
||||
}
|
||||
|
||||
bool XYThetaIterator::hasMoreTwists()
|
||||
{
|
||||
return x_it_ && !x_it_->isFinished();
|
||||
}
|
||||
|
||||
nav_2d_msgs::msg::Twist2D XYThetaIterator::nextTwist()
|
||||
{
|
||||
nav_2d_msgs::msg::Twist2D velocity;
|
||||
velocity.x = x_it_->getVelocity();
|
||||
velocity.y = y_it_->getVelocity();
|
||||
velocity.theta = th_it_->getVelocity();
|
||||
|
||||
iterateToValidVelocity();
|
||||
|
||||
return velocity;
|
||||
}
|
||||
|
||||
void XYThetaIterator::iterateToValidVelocity()
|
||||
{
|
||||
bool valid = false;
|
||||
while (!valid && hasMoreTwists()) {
|
||||
++(*th_it_);
|
||||
if (th_it_->isFinished()) {
|
||||
th_it_->reset();
|
||||
++(*y_it_);
|
||||
if (y_it_->isFinished()) {
|
||||
y_it_->reset();
|
||||
++(*x_it_);
|
||||
}
|
||||
}
|
||||
valid = isValidVelocity();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace dwb_plugins
|
||||
Reference in New Issue
Block a user