add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,64 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(dwb_plugins)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(angles REQUIRED)
|
||||
find_package(dwb_core REQUIRED)
|
||||
find_package(nav_2d_msgs REQUIRED)
|
||||
find_package(nav_2d_utils REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
|
||||
nav2_package()
|
||||
|
||||
set(dependencies
|
||||
angles
|
||||
dwb_core
|
||||
nav_2d_msgs
|
||||
nav_2d_utils
|
||||
pluginlib
|
||||
rclcpp
|
||||
nav2_util
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
add_library(standard_traj_generator SHARED
|
||||
src/standard_traj_generator.cpp
|
||||
src/limited_accel_generator.cpp
|
||||
src/kinematic_parameters.cpp
|
||||
src/xy_theta_iterator.cpp)
|
||||
ament_target_dependencies(standard_traj_generator ${dependencies})
|
||||
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
add_subdirectory(test)
|
||||
endif()
|
||||
|
||||
install(TARGETS standard_traj_generator
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
install(FILES plugins.xml
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(standard_traj_generator)
|
||||
pluginlib_export_plugin_description_file(dwb_core plugins.xml)
|
||||
|
||||
ament_package()
|
||||
+138
@@ -0,0 +1,138 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
|
||||
#define DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct KinematicParameters
|
||||
* @brief A struct containing one representation of the robot's kinematics
|
||||
*/
|
||||
struct KinematicParameters
|
||||
{
|
||||
friend class KinematicsHandler;
|
||||
|
||||
inline double getMinX() {return min_vel_x_;}
|
||||
inline double getMaxX() {return max_vel_x_;}
|
||||
inline double getAccX() {return acc_lim_x_;}
|
||||
inline double getDecelX() {return decel_lim_x_;}
|
||||
|
||||
inline double getMinY() {return min_vel_y_;}
|
||||
inline double getMaxY() {return max_vel_y_;}
|
||||
inline double getAccY() {return acc_lim_y_;}
|
||||
inline double getDecelY() {return decel_lim_y_;}
|
||||
|
||||
inline double getMinSpeedXY() {return min_speed_xy_;}
|
||||
inline double getMaxSpeedXY() {return max_speed_xy_;}
|
||||
|
||||
inline double getMinTheta() {return -max_vel_theta_;}
|
||||
inline double getMaxTheta() {return max_vel_theta_;}
|
||||
inline double getAccTheta() {return acc_lim_theta_;}
|
||||
inline double getDecelTheta() {return decel_lim_theta_;}
|
||||
inline double getMinSpeedTheta() {return min_speed_theta_;}
|
||||
|
||||
inline double getMinSpeedXY_SQ() {return min_speed_xy_sq_;}
|
||||
inline double getMaxSpeedXY_SQ() {return max_speed_xy_sq_;}
|
||||
|
||||
protected:
|
||||
// For parameter descriptions, see cfg/KinematicParams.cfg
|
||||
double min_vel_x_{0};
|
||||
double min_vel_y_{0};
|
||||
double max_vel_x_{0};
|
||||
double max_vel_y_{0};
|
||||
double base_max_vel_x_{0};
|
||||
double base_max_vel_y_{0};
|
||||
double max_vel_theta_{0};
|
||||
double base_max_vel_theta_{0};
|
||||
double min_speed_xy_{0};
|
||||
double max_speed_xy_{0};
|
||||
double base_max_speed_xy_{0};
|
||||
double min_speed_theta_{0};
|
||||
double acc_lim_x_{0};
|
||||
double acc_lim_y_{0};
|
||||
double acc_lim_theta_{0};
|
||||
double decel_lim_x_{0};
|
||||
double decel_lim_y_{0};
|
||||
double decel_lim_theta_{0};
|
||||
|
||||
// Cached square values of min_speed_xy and max_speed_xy
|
||||
double min_speed_xy_sq_{0};
|
||||
double max_speed_xy_sq_{0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @class KinematicsHandler
|
||||
* @brief A class managing the representation of the robot's kinematics
|
||||
*/
|
||||
class KinematicsHandler
|
||||
{
|
||||
public:
|
||||
KinematicsHandler();
|
||||
~KinematicsHandler();
|
||||
void initialize(const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name);
|
||||
|
||||
inline KinematicParameters getKinematics() {return *kinematics_.load();}
|
||||
|
||||
void setSpeedLimit(const double & speed_limit, const bool & percentage);
|
||||
|
||||
using Ptr = std::shared_ptr<KinematicsHandler>;
|
||||
|
||||
protected:
|
||||
std::atomic<KinematicParameters *> kinematics_;
|
||||
|
||||
// Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
/**
|
||||
* @brief Callback executed when a paramter change is detected
|
||||
* @param parameters list of changed parameters
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
|
||||
void update_kinematics(KinematicParameters kinematics);
|
||||
std::string plugin_name_;
|
||||
};
|
||||
|
||||
} // namespace dwb_plugins
|
||||
|
||||
#endif // DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
|
||||
+78
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
|
||||
#define DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "dwb_plugins/standard_traj_generator.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
/**
|
||||
* @class LimitedAccelGenerator
|
||||
* @brief Limits the acceleration in the generated trajectories to a fraction of the simulated time.
|
||||
*/
|
||||
class LimitedAccelGenerator : public StandardTrajectoryGenerator
|
||||
{
|
||||
public:
|
||||
void initialize(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh,
|
||||
const std::string & plugin_name) override;
|
||||
void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity) override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
|
||||
*
|
||||
* Unlike the StandardTrajectoryGenerator, the velocity remains constant in the LimitedAccelGenerator
|
||||
*
|
||||
* @param cmd_vel Desired velocity
|
||||
* @param start_vel starting velocity
|
||||
* @param dt amount of time in seconds
|
||||
* @return cmd_vel
|
||||
*/
|
||||
nav_2d_msgs::msg::Twist2D computeNewVelocity(
|
||||
const nav_2d_msgs::msg::Twist2D & cmd_vel,
|
||||
const nav_2d_msgs::msg::Twist2D & start_vel,
|
||||
const double dt) override;
|
||||
double acceleration_time_;
|
||||
std::string plugin_name_;
|
||||
};
|
||||
} // namespace dwb_plugins
|
||||
|
||||
#endif // DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
|
||||
+170
@@ -0,0 +1,170 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
|
||||
#define DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
|
||||
const double EPSILON = 1E-5;
|
||||
|
||||
/**
|
||||
* @brief Given initial conditions and a time, figure out the end velocity
|
||||
*
|
||||
* @param v0 Initial velocity
|
||||
* @param accel The acceleration rate
|
||||
* @param decel The decceleration rate
|
||||
* @param dt Delta time - amount of time to project into the future
|
||||
* @param target target velocity
|
||||
* @return The velocity dt seconds after v0.
|
||||
*/
|
||||
inline double projectVelocity(double v0, double accel, double decel, double dt, double target)
|
||||
{
|
||||
double v1;
|
||||
if (v0 < target) {
|
||||
v1 = v0 + accel * dt;
|
||||
return std::min(target, v1);
|
||||
} else {
|
||||
v1 = v0 + decel * dt;
|
||||
return std::max(target, v1);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @class OneDVelocityIterator
|
||||
* @brief An iterator for generating a number of samples in a range
|
||||
*
|
||||
* In its simplest usage, this gives us N (num_samples) different velocities that are reachable
|
||||
* given our current velocity. However, there is some fancy logic around zero velocities and
|
||||
* the min/max velocities
|
||||
*
|
||||
* If the current velocity is 2 m/s, and the acceleration limit is 1 m/ss and the acc_time is 1 s,
|
||||
* this class would provide velocities between 1 m/s and 3 m/s.
|
||||
*
|
||||
*
|
||||
*
|
||||
*/
|
||||
class OneDVelocityIterator
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for the velocity iterator
|
||||
*
|
||||
* @param current Current velocity
|
||||
* @param min Minimum velocity allowable
|
||||
* @param max Maximum velocity allowable
|
||||
* @param acc_limit Acceleration Limit
|
||||
* @param decel_limit Deceleration Limit
|
||||
* @param num_samples The number of samples to return
|
||||
*/
|
||||
OneDVelocityIterator(
|
||||
double current, double min, double max, double acc_limit, double decel_limit, double acc_time,
|
||||
int num_samples)
|
||||
{
|
||||
if (current < min) {
|
||||
current = min;
|
||||
} else if (current > max) {
|
||||
current = max;
|
||||
}
|
||||
max_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, max);
|
||||
min_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, min);
|
||||
reset();
|
||||
|
||||
if (fabs(min_vel_ - max_vel_) < EPSILON) {
|
||||
increment_ = 1.0;
|
||||
return;
|
||||
}
|
||||
num_samples = std::max(2, num_samples);
|
||||
|
||||
// e.g. for 4 samples, split distance in 3 even parts
|
||||
increment_ = (max_vel_ - min_vel_) / std::max(1, (num_samples - 1));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the next velocity available
|
||||
*/
|
||||
double getVelocity() const
|
||||
{
|
||||
if (return_zero_now_) {return 0.0;}
|
||||
return current_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Increment the iterator
|
||||
*/
|
||||
OneDVelocityIterator & operator++()
|
||||
{
|
||||
if (return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 &&
|
||||
current_ + increment_ <= max_vel_ + EPSILON)
|
||||
{
|
||||
return_zero_now_ = true;
|
||||
return_zero_ = false;
|
||||
} else {
|
||||
current_ += increment_;
|
||||
return_zero_now_ = false;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Reset back to the first velocity
|
||||
*/
|
||||
void reset()
|
||||
{
|
||||
current_ = min_vel_;
|
||||
return_zero_ = true;
|
||||
return_zero_now_ = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* If we have returned all the velocities for this iteration
|
||||
*/
|
||||
bool isFinished() const
|
||||
{
|
||||
return current_ > max_vel_ + EPSILON;
|
||||
}
|
||||
|
||||
private:
|
||||
bool return_zero_, return_zero_now_;
|
||||
double min_vel_, max_vel_;
|
||||
double current_;
|
||||
double increment_;
|
||||
};
|
||||
} // namespace dwb_plugins
|
||||
|
||||
#endif // DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
|
||||
+172
@@ -0,0 +1,172 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
|
||||
#define DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "dwb_core/trajectory_generator.hpp"
|
||||
#include "dwb_plugins/velocity_iterator.hpp"
|
||||
#include "dwb_plugins/kinematic_parameters.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
|
||||
/**
|
||||
* @class StandardTrajectoryGenerator
|
||||
* @brief Standard DWA-like trajectory generator.
|
||||
*/
|
||||
class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator
|
||||
{
|
||||
public:
|
||||
// Standard TrajectoryGenerator interface
|
||||
void initialize(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh,
|
||||
const std::string & plugin_name) override;
|
||||
void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity) override;
|
||||
bool hasMoreTwists() override;
|
||||
nav_2d_msgs::msg::Twist2D nextTwist() override;
|
||||
|
||||
dwb_msgs::msg::Trajectory2D generateTrajectory(
|
||||
const geometry_msgs::msg::Pose2D & start_pose,
|
||||
const nav_2d_msgs::msg::Twist2D & start_vel,
|
||||
const nav_2d_msgs::msg::Twist2D & cmd_vel) override;
|
||||
|
||||
/**
|
||||
* @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) override
|
||||
{
|
||||
if (kinematics_handler_) {
|
||||
kinematics_handler_->setSpeedLimit(speed_limit, percentage);
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding
|
||||
*/
|
||||
virtual void initializeIterator(const nav2_util::LifecycleNode::SharedPtr & nh);
|
||||
|
||||
/**
|
||||
* @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
|
||||
*
|
||||
* @param cmd_vel Desired velocity
|
||||
* @param start_vel starting velocity
|
||||
* @param dt amount of time in seconds
|
||||
* @return new velocity after dt seconds
|
||||
*/
|
||||
virtual nav_2d_msgs::msg::Twist2D computeNewVelocity(
|
||||
const nav_2d_msgs::msg::Twist2D & cmd_vel, const nav_2d_msgs::msg::Twist2D & start_vel,
|
||||
const double dt);
|
||||
|
||||
/**
|
||||
* @brief Use the robot's kinematic model to predict new positions for the robot
|
||||
*
|
||||
* @param start_pose Starting pose
|
||||
* @param vel Actual robot velocity (assumed to be within acceleration limits)
|
||||
* @param dt amount of time in seconds
|
||||
* @return New pose after dt seconds
|
||||
*/
|
||||
virtual geometry_msgs::msg::Pose2D computeNewPosition(
|
||||
const geometry_msgs::msg::Pose2D start_pose, const nav_2d_msgs::msg::Twist2D & vel,
|
||||
const double dt);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Compute an array of time deltas between the points in the generated trajectory.
|
||||
*
|
||||
* @param cmd_vel The desired command velocity
|
||||
* @return vector of the difference between each time step in the generated trajectory
|
||||
*
|
||||
* If we are discretizing by time, the returned vector will be the same constant time_granularity
|
||||
* for all cmd_vels. Otherwise, you will get times based on the linear/angular granularity.
|
||||
*
|
||||
* Right now the vector contains a single value repeated many times, but this method could be overridden
|
||||
* to allow for dynamic spacing
|
||||
*/
|
||||
virtual std::vector<double> getTimeSteps(const nav_2d_msgs::msg::Twist2D & cmd_vel);
|
||||
|
||||
KinematicsHandler::Ptr kinematics_handler_;
|
||||
std::shared_ptr<VelocityIterator> velocity_iterator_;
|
||||
|
||||
double sim_time_;
|
||||
|
||||
// Sampling Parameters
|
||||
bool discretize_by_time_;
|
||||
|
||||
/// @brief If discretizing by time, the amount of time between each point in the traj
|
||||
double time_granularity_;
|
||||
|
||||
/// @brief If not discretizing by time, the amount of linear space between points
|
||||
double linear_granularity_;
|
||||
|
||||
/// @brief If not discretizing by time, the amount of angular space between points
|
||||
double angular_granularity_;
|
||||
|
||||
/// @brief the name of the overlying plugin ID
|
||||
std::string plugin_name_;
|
||||
|
||||
/// @brief Option to limit velocity in the trajectory generator by using current velocity
|
||||
bool limit_vel_cmd_in_traj_;
|
||||
|
||||
/* Backwards Compatibility Parameter: include_last_point
|
||||
*
|
||||
* dwa had an off-by-one error built into it.
|
||||
* It generated N trajectory points, where N = ceil(sim_time / time_delta).
|
||||
* If for example, sim_time=3.0 and time_delta=1.5, it would generate trajectories with 2 points, which
|
||||
* indeed were time_delta seconds apart. However, the points would be at t=0 and t=1.5, and thus the
|
||||
* actual sim_time was much less than advertised.
|
||||
*
|
||||
* This is remedied by adding one final point at t=sim_time, but only if include_last_point_ is true.
|
||||
*
|
||||
* Nothing I could find actually used the time_delta variable or seemed to care that the trajectories
|
||||
* were not projected out as far as they intended.
|
||||
*/
|
||||
bool include_last_point_;
|
||||
};
|
||||
|
||||
|
||||
} // namespace dwb_plugins
|
||||
|
||||
#endif // DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
|
||||
@@ -0,0 +1,62 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
|
||||
#define DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav_2d_msgs/msg/twist2_d.hpp"
|
||||
#include "dwb_plugins/kinematic_parameters.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
class VelocityIterator
|
||||
{
|
||||
public:
|
||||
virtual ~VelocityIterator() {}
|
||||
virtual void initialize(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh,
|
||||
KinematicsHandler::Ptr kinematics,
|
||||
const std::string & plugin_name) = 0;
|
||||
virtual void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) = 0;
|
||||
virtual bool hasMoreTwists() = 0;
|
||||
virtual nav_2d_msgs::msg::Twist2D nextTwist() = 0;
|
||||
};
|
||||
} // namespace dwb_plugins
|
||||
|
||||
#endif // DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
|
||||
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
|
||||
#define DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "dwb_plugins/velocity_iterator.hpp"
|
||||
#include "dwb_plugins/one_d_velocity_iterator.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
|
||||
namespace dwb_plugins
|
||||
{
|
||||
class XYThetaIterator : public VelocityIterator
|
||||
{
|
||||
public:
|
||||
XYThetaIterator()
|
||||
: kinematics_handler_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {}
|
||||
void initialize(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh,
|
||||
KinematicsHandler::Ptr kinematics,
|
||||
const std::string & plugin_name) override;
|
||||
void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) override;
|
||||
bool hasMoreTwists() override;
|
||||
nav_2d_msgs::msg::Twist2D nextTwist() override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Check to see whether the combined x/y/theta velocities are valid
|
||||
* @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits
|
||||
*
|
||||
* This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta.
|
||||
* The speed is valid if
|
||||
* 1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative)
|
||||
* AND
|
||||
* 2) min_speed_xy is negative or min_speed_theta is negative or
|
||||
* hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta.
|
||||
*/
|
||||
bool isValidSpeed(double x, double y, double theta);
|
||||
virtual bool isValidVelocity();
|
||||
void iterateToValidVelocity();
|
||||
int vx_samples_, vy_samples_, vtheta_samples_;
|
||||
KinematicsHandler::Ptr kinematics_handler_;
|
||||
|
||||
std::shared_ptr<OneDVelocityIterator> x_it_, y_it_, th_it_;
|
||||
};
|
||||
} // namespace dwb_plugins
|
||||
|
||||
#endif // DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
|
||||
@@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>dwb_plugins</name>
|
||||
<version>1.1.18</version>
|
||||
<description>
|
||||
Standard implementations of the GoalChecker
|
||||
and TrajectoryGenerators for dwb_core
|
||||
</description>
|
||||
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
|
||||
<license>BSD-3-Clause</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>nav2_common</build_depend>
|
||||
|
||||
<depend>angles</depend>
|
||||
<depend>dwb_core</depend>
|
||||
<depend>nav_2d_msgs</depend>
|
||||
<depend>nav_2d_utils</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>nav2_util</depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,20 @@
|
||||
<class_libraries>
|
||||
<library path="simple_goal_checker">
|
||||
<class type="dwb_plugins::SimpleGoalChecker" base_class_type="nav2_core::GoalChecker">
|
||||
<description></description>
|
||||
</class>
|
||||
</library>
|
||||
<library path="standard_traj_generator">
|
||||
<class type="dwb_plugins::StandardTrajectoryGenerator" base_class_type="dwb_core::TrajectoryGenerator">
|
||||
<description></description>
|
||||
</class>
|
||||
<class type="dwb_plugins::LimitedAccelGenerator" base_class_type="dwb_core::TrajectoryGenerator">
|
||||
<description></description>
|
||||
</class>
|
||||
</library>
|
||||
<library path="stopped_goal_checker">
|
||||
<class type="dwb_plugins::StoppedGoalChecker" base_class_type="nav2_core::GoalChecker">
|
||||
<description></description>
|
||||
</class>
|
||||
</library>
|
||||
</class_libraries>
|
||||
@@ -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
|
||||
@@ -0,0 +1,10 @@
|
||||
ament_add_gtest(vtest velocity_iterator_test.cpp)
|
||||
|
||||
ament_add_gtest(twist_gen_test twist_gen.cpp)
|
||||
target_link_libraries(twist_gen_test standard_traj_generator)
|
||||
|
||||
ament_add_gtest(kinematic_parameters_test kinematic_parameters_test.cpp)
|
||||
target_link_libraries(kinematic_parameters_test standard_traj_generator)
|
||||
|
||||
ament_add_gtest(speed_limit_test speed_limit_test.cpp)
|
||||
target_link_libraries(speed_limit_test standard_traj_generator)
|
||||
@@ -0,0 +1,129 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* 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 <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "dwb_plugins/kinematic_parameters.hpp"
|
||||
|
||||
using rcl_interfaces::msg::Parameter;
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using rcl_interfaces::msg::ParameterEvent;
|
||||
|
||||
class KinematicsHandlerTest : public dwb_plugins::KinematicsHandler
|
||||
{
|
||||
public:
|
||||
void simulate_event(
|
||||
std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
dynamicParametersCallback(parameters);
|
||||
}
|
||||
};
|
||||
|
||||
TEST(KinematicParameters, SetAllParameters) {
|
||||
std::string nodeName = "test_node";
|
||||
auto node = nav2_util::LifecycleNode::make_shared(nodeName);
|
||||
KinematicsHandlerTest kh;
|
||||
kh.initialize(node, nodeName);
|
||||
|
||||
std::vector<rclcpp::Parameter> parameters;
|
||||
rclcpp::Parameter
|
||||
p_minX(nodeName + ".min_vel_x", 12.34),
|
||||
p_maxX(nodeName + ".max_vel_x", 23.45),
|
||||
p_minY(nodeName + ".min_vel_y", 34.56),
|
||||
p_maxY(nodeName + ".max_vel_y", 45.67),
|
||||
p_accX(nodeName + ".acc_lim_x", 56.78),
|
||||
p_decelX(nodeName + ".acc_lim_y", 67.89),
|
||||
p_accY(nodeName + ".decel_lim_x", 78.90),
|
||||
p_decelY(nodeName + ".decel_lim_y", 89.01),
|
||||
p_minSpeedXY(nodeName + ".min_speed_xy", 90.12),
|
||||
p_maxSpeedXY(nodeName + ".max_speed_xy", 123.456),
|
||||
p_maxTheta(nodeName + ".max_vel_theta", 345.678),
|
||||
p_accTheta(nodeName + ".acc_lim_theta", 234.567),
|
||||
p_decelTheta(nodeName + ".decel_lim_theta", 456.789),
|
||||
p_minSpeedTheta(nodeName + ".min_speed_theta", 567.890);
|
||||
|
||||
parameters.push_back(p_minX);
|
||||
parameters.push_back(p_minX);
|
||||
parameters.push_back(p_maxX);
|
||||
parameters.push_back(p_minY);
|
||||
parameters.push_back(p_maxY);
|
||||
parameters.push_back(p_accX);
|
||||
parameters.push_back(p_accY);
|
||||
parameters.push_back(p_decelX);
|
||||
parameters.push_back(p_decelY);
|
||||
parameters.push_back(p_minSpeedXY);
|
||||
parameters.push_back(p_maxSpeedXY);
|
||||
parameters.push_back(p_maxTheta);
|
||||
parameters.push_back(p_accTheta);
|
||||
parameters.push_back(p_decelTheta);
|
||||
parameters.push_back(p_minSpeedTheta);
|
||||
|
||||
kh.simulate_event(parameters);
|
||||
|
||||
dwb_plugins::KinematicParameters kp = kh.getKinematics();
|
||||
|
||||
EXPECT_EQ(kp.getMinX(), 12.34);
|
||||
EXPECT_EQ(kp.getMaxX(), 23.45);
|
||||
EXPECT_EQ(kp.getMinY(), 34.56);
|
||||
EXPECT_EQ(kp.getMaxY(), 45.67);
|
||||
EXPECT_EQ(kp.getAccX(), 56.78);
|
||||
EXPECT_EQ(kp.getAccY(), 67.89);
|
||||
EXPECT_EQ(kp.getDecelX(), 78.90);
|
||||
EXPECT_EQ(kp.getDecelY(), 89.01);
|
||||
EXPECT_EQ(kp.getMinSpeedXY(), 90.12);
|
||||
EXPECT_EQ(kp.getMaxSpeedXY(), 123.456);
|
||||
EXPECT_EQ(kp.getAccTheta(), 234.567);
|
||||
EXPECT_EQ(kp.getMaxTheta(), 345.678);
|
||||
EXPECT_EQ(kp.getDecelTheta(), 456.789);
|
||||
EXPECT_EQ(kp.getMinSpeedTheta(), 567.890);
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,171 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2020, Samsung Research Russia
|
||||
* 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.
|
||||
*
|
||||
* Author: Alexey Merzlyakov
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
|
||||
|
||||
#include "dwb_plugins/kinematic_parameters.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
static constexpr double EPSILON = 1e-5;
|
||||
|
||||
static const char NODE_NAME[] = "test_node";
|
||||
static const double MAX_VEL_X = 40.0;
|
||||
static const double MAX_VEL_Y = 30.0;
|
||||
static const double MAX_VEL_THETA = 15.0;
|
||||
static const double MAX_VEL_LINEAR = 50.0;
|
||||
|
||||
class TestNode : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
TestNode()
|
||||
{
|
||||
const std::string node_name = NODE_NAME;
|
||||
node_ = nav2_util::LifecycleNode::make_shared(node_name);
|
||||
|
||||
node_->declare_parameter(
|
||||
node_name + ".max_vel_x", rclcpp::ParameterValue(MAX_VEL_X));
|
||||
node_->set_parameter(
|
||||
rclcpp::Parameter(node_name + ".max_vel_x", MAX_VEL_X));
|
||||
|
||||
node_->declare_parameter(
|
||||
node_name + ".max_vel_y", rclcpp::ParameterValue(MAX_VEL_Y));
|
||||
node_->set_parameter(
|
||||
rclcpp::Parameter(node_name + ".max_vel_y", MAX_VEL_Y));
|
||||
|
||||
node_->declare_parameter(
|
||||
node_name + ".max_vel_theta", rclcpp::ParameterValue(MAX_VEL_THETA));
|
||||
node_->set_parameter(
|
||||
rclcpp::Parameter(node_name + ".max_vel_theta", MAX_VEL_THETA));
|
||||
|
||||
node_->declare_parameter(
|
||||
node_name + ".max_speed_xy", rclcpp::ParameterValue(MAX_VEL_LINEAR));
|
||||
node_->set_parameter(
|
||||
rclcpp::Parameter(node_name + ".max_speed_xy", MAX_VEL_LINEAR));
|
||||
}
|
||||
|
||||
~TestNode() {}
|
||||
|
||||
protected:
|
||||
nav2_util::LifecycleNode::SharedPtr node_;
|
||||
};
|
||||
|
||||
TEST_F(TestNode, TestPercentLimit)
|
||||
{
|
||||
dwb_plugins::KinematicsHandler kh;
|
||||
kh.initialize(node_, NODE_NAME);
|
||||
|
||||
dwb_plugins::KinematicParameters kp = kh.getKinematics();
|
||||
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON);
|
||||
|
||||
// Set speed limit 30% from maximum robot speed
|
||||
kh.setSpeedLimit(30, true);
|
||||
|
||||
// Update KinematicParameters values from KinematicsHandler
|
||||
kp = kh.getKinematics();
|
||||
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X * 0.3, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y * 0.3, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA * 0.3, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR * 0.3, EPSILON);
|
||||
|
||||
// Restore maximum speed to its default
|
||||
kh.setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, true);
|
||||
|
||||
// Update KinematicParameters values from KinematicsHandler
|
||||
kp = kh.getKinematics();
|
||||
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON);
|
||||
}
|
||||
|
||||
TEST_F(TestNode, TestAbsoluteLimit)
|
||||
{
|
||||
dwb_plugins::KinematicsHandler kh;
|
||||
kh.initialize(node_, NODE_NAME);
|
||||
|
||||
dwb_plugins::KinematicParameters kp = kh.getKinematics();
|
||||
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON);
|
||||
|
||||
// Set speed limit 35.0 m/s
|
||||
kh.setSpeedLimit(35.0, false);
|
||||
|
||||
// Update KinematicParameters values from KinematicsHandler
|
||||
kp = kh.getKinematics();
|
||||
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X * 35.0 / MAX_VEL_LINEAR, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y * 35.0 / MAX_VEL_LINEAR, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA * 35.0 / MAX_VEL_LINEAR, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxSpeedXY(), 35.0, EPSILON);
|
||||
|
||||
// Restore maximum speed to its default
|
||||
kh.setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, false);
|
||||
|
||||
// Update KinematicParameters values from KinematicsHandler
|
||||
kp = kh.getKinematics();
|
||||
EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON);
|
||||
EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
// Initialize the system
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
// Actual testing
|
||||
bool test_result = RUN_ALL_TESTS();
|
||||
|
||||
// Shutdown
|
||||
rclcpp::shutdown();
|
||||
|
||||
return test_result;
|
||||
}
|
||||
@@ -0,0 +1,503 @@
|
||||
/*
|
||||
* 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 <cmath>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "dwb_plugins/standard_traj_generator.hpp"
|
||||
#include "dwb_plugins/limited_accel_generator.hpp"
|
||||
#include "dwb_core/exceptions.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
using std::hypot;
|
||||
using std::fabs;
|
||||
using dwb_plugins::StandardTrajectoryGenerator;
|
||||
|
||||
geometry_msgs::msg::Pose2D origin;
|
||||
nav_2d_msgs::msg::Twist2D zero;
|
||||
nav_2d_msgs::msg::Twist2D forward;
|
||||
|
||||
class LimitedAccelGeneratorTest : public dwb_plugins::LimitedAccelGenerator
|
||||
{
|
||||
public:
|
||||
double getAccelerationTime()
|
||||
{
|
||||
return acceleration_time_;
|
||||
}
|
||||
};
|
||||
|
||||
std::vector<rclcpp::Parameter> getDefaultKinematicParameters()
|
||||
{
|
||||
std::vector<rclcpp::Parameter> parameters;
|
||||
parameters.push_back(rclcpp::Parameter("dwb.min_vel_x", 0.0));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.max_vel_x", 0.55));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.min_vel_y", -0.1));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.max_vel_y", 0.1));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.max_vel_theta", 1.0));
|
||||
|
||||
parameters.push_back(rclcpp::Parameter("dwb.acc_lim_x", 2.5));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.acc_lim_y", 2.5));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.acc_lim_theta", 3.2));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.decel_lim_x", -2.5));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.decel_lim_y", -2.5));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.decel_lim_theta", -3.2));
|
||||
|
||||
parameters.push_back(rclcpp::Parameter("dwb.min_speed_xy", 0.1));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.max_speed_xy", 0.55));
|
||||
parameters.push_back(rclcpp::Parameter("dwb.min_speed_theta", 0.4));
|
||||
|
||||
return parameters;
|
||||
}
|
||||
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(
|
||||
const std::string & name,
|
||||
const std::vector<rclcpp::Parameter> & overrides = {})
|
||||
{
|
||||
rclcpp::NodeOptions node_options;
|
||||
node_options.parameter_overrides(getDefaultKinematicParameters());
|
||||
node_options.parameter_overrides().insert(
|
||||
node_options.parameter_overrides().end(), overrides.begin(), overrides.end());
|
||||
|
||||
auto node = rclcpp_lifecycle::LifecycleNode::make_shared(name, node_options);
|
||||
node->on_configure(node->get_current_state());
|
||||
node->on_activate(node->get_current_state());
|
||||
|
||||
return node;
|
||||
}
|
||||
|
||||
void checkLimits(
|
||||
const std::vector<nav_2d_msgs::msg::Twist2D> & twists,
|
||||
double exp_min_x, double exp_max_x, double exp_min_y, double exp_max_y,
|
||||
double exp_min_theta, double exp_max_theta,
|
||||
double exp_max_xy = -1.0,
|
||||
double exp_min_xy = -1.0, double exp_min_speed_theta = -1.0)
|
||||
{
|
||||
ASSERT_GT(twists.size(), 0u);
|
||||
nav_2d_msgs::msg::Twist2D first = twists[0];
|
||||
|
||||
double min_x = first.x, max_x = first.x, min_y = first.y, max_y = first.y;
|
||||
double min_theta = first.theta, max_theta = first.theta;
|
||||
double max_xy = hypot(first.x, first.y);
|
||||
|
||||
for (nav_2d_msgs::msg::Twist2D twist : twists) {
|
||||
min_x = std::min(min_x, twist.x);
|
||||
min_y = std::min(min_y, twist.y);
|
||||
min_theta = std::min(min_theta, twist.theta);
|
||||
max_x = std::max(max_x, twist.x);
|
||||
max_y = std::max(max_y, twist.y);
|
||||
max_theta = std::max(max_theta, twist.theta);
|
||||
double hyp = hypot(twist.x, twist.y);
|
||||
max_xy = std::max(max_xy, hyp);
|
||||
|
||||
if (exp_min_xy >= 0 && exp_min_speed_theta >= 0) {
|
||||
EXPECT_TRUE(fabs(twist.theta) >= exp_min_speed_theta || hyp >= exp_min_xy);
|
||||
}
|
||||
}
|
||||
EXPECT_DOUBLE_EQ(min_x, exp_min_x);
|
||||
EXPECT_DOUBLE_EQ(max_x, exp_max_x);
|
||||
EXPECT_DOUBLE_EQ(min_y, exp_min_y);
|
||||
EXPECT_DOUBLE_EQ(max_y, exp_max_y);
|
||||
EXPECT_DOUBLE_EQ(min_theta, exp_min_theta);
|
||||
EXPECT_DOUBLE_EQ(max_theta, exp_max_theta);
|
||||
if (exp_max_xy >= 0) {
|
||||
EXPECT_DOUBLE_EQ(max_xy, exp_max_xy);
|
||||
}
|
||||
}
|
||||
|
||||
double durationToSec(builtin_interfaces::msg::Duration d)
|
||||
{
|
||||
return d.sec + d.nanosec * 1e-9;
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, standard_gen)
|
||||
{
|
||||
auto nh = makeTestNode("st_gen");
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
|
||||
EXPECT_EQ(twists.size(), 1926u);
|
||||
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, 0.55, 0.1, 0.4);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, max_xy)
|
||||
{
|
||||
auto nh = makeTestNode("max_xy", {rclcpp::Parameter("dwb.max_speed_xy", 1.0)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
|
||||
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
|
||||
// Expect more twists since max_speed_xy is now beyond feasible limits
|
||||
EXPECT_EQ(twists.size(), 2010u);
|
||||
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1));
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, min_xy)
|
||||
{
|
||||
auto nh = makeTestNode("min_xy", {rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
|
||||
// Expect even more since theres no min_speed_xy
|
||||
EXPECT_EQ(twists.size(), 2015u);
|
||||
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, min_theta)
|
||||
{
|
||||
auto nh = makeTestNode("min_theta", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
|
||||
// Expect even more since theres no min_speed_xy
|
||||
EXPECT_EQ(twists.size(), 2015u);
|
||||
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, no_limits)
|
||||
{
|
||||
auto nh = makeTestNode(
|
||||
"no_limits", {
|
||||
rclcpp::Parameter("dwb.max_speed_xy", -1.0),
|
||||
rclcpp::Parameter("dwb.min_speed_xy", -1.0),
|
||||
rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
|
||||
// vx_samples * vtheta_samples * vy_samples + added zero theta samples - (0,0,0)
|
||||
EXPECT_EQ(twists.size(), 20u * 20u * 5u + 100u - 1u);
|
||||
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, no_limits_samples)
|
||||
{
|
||||
const int x_samples = 10, y_samples = 3, theta_samples = 5;
|
||||
auto nh = makeTestNode(
|
||||
"no_limits_samples", {
|
||||
rclcpp::Parameter("dwb.max_speed_xy", -1.0),
|
||||
rclcpp::Parameter("dwb.min_speed_xy", -1.0),
|
||||
rclcpp::Parameter("dwb.min_speed_theta", -1.0),
|
||||
rclcpp::Parameter("dwb.vx_samples", x_samples),
|
||||
rclcpp::Parameter("dwb.vy_samples", y_samples),
|
||||
rclcpp::Parameter("dwb.vtheta_samples", theta_samples)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
|
||||
EXPECT_EQ(twists.size(), static_cast<unsigned>(x_samples * y_samples * theta_samples - 1));
|
||||
checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, dwa_gen)
|
||||
{
|
||||
auto nh = makeTestNode("dwa_gen", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
|
||||
dwb_plugins::LimitedAccelGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
|
||||
// Same as no-limits since everything is within our velocity limits
|
||||
EXPECT_EQ(twists.size(), 20u * 20u * 5u + 100u - 1u);
|
||||
checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, dwa_gen_zero_frequency)
|
||||
{
|
||||
auto nh = makeTestNode("dwa_gen");
|
||||
nh->declare_parameter("controller_frequency", 0.0);
|
||||
LimitedAccelGeneratorTest gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
// Default value should be 0.05
|
||||
EXPECT_EQ(gen.getAccelerationTime(), 0.05);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, dwa_gen_one_frequency)
|
||||
{
|
||||
auto nh = makeTestNode("dwa_gen");
|
||||
nh->declare_parameter("controller_frequency", 1.0);
|
||||
LimitedAccelGeneratorTest gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
EXPECT_EQ(gen.getAccelerationTime(), 1.0);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, dwa_gen_ten_frequency)
|
||||
{
|
||||
auto nh = makeTestNode("dwa_gen");
|
||||
nh->declare_parameter("controller_frequency", 10.0);
|
||||
LimitedAccelGeneratorTest gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
EXPECT_EQ(gen.getAccelerationTime(), 0.1);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, dwa_gen_fifty_frequency)
|
||||
{
|
||||
auto nh = makeTestNode("dwa_gen");
|
||||
nh->declare_parameter("controller_frequency", 50.0);
|
||||
LimitedAccelGeneratorTest gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
EXPECT_EQ(gen.getAccelerationTime(), 0.02);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, dwa_gen_hundred_frequency)
|
||||
{
|
||||
auto nh = makeTestNode("dwa_gen");
|
||||
nh->declare_parameter("controller_frequency", 100.0);
|
||||
LimitedAccelGeneratorTest gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
EXPECT_EQ(gen.getAccelerationTime(), 0.01);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, nonzero)
|
||||
{
|
||||
auto nh = makeTestNode("nonzero", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
|
||||
dwb_plugins::LimitedAccelGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
nav_2d_msgs::msg::Twist2D initial;
|
||||
initial.x = 0.1;
|
||||
initial.y = -0.08;
|
||||
initial.theta = 0.05;
|
||||
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(initial);
|
||||
EXPECT_EQ(twists.size(), 2519u);
|
||||
checkLimits(
|
||||
twists, 0.0, 0.225, -0.1, 0.045, -0.11000000000000003, 0.21,
|
||||
0.24622144504490268, 0.0, 0.1);
|
||||
}
|
||||
|
||||
void matchPose(const geometry_msgs::msg::Pose2D & a, const geometry_msgs::msg::Pose2D & b)
|
||||
{
|
||||
EXPECT_DOUBLE_EQ(a.x, b.x);
|
||||
EXPECT_DOUBLE_EQ(a.y, b.y);
|
||||
EXPECT_DOUBLE_EQ(a.theta, b.theta);
|
||||
}
|
||||
|
||||
void matchPose(
|
||||
const geometry_msgs::msg::Pose2D & a, const double x, const double y,
|
||||
const double theta)
|
||||
{
|
||||
EXPECT_DOUBLE_EQ(a.x, x);
|
||||
EXPECT_DOUBLE_EQ(a.y, y);
|
||||
EXPECT_DOUBLE_EQ(a.theta, theta);
|
||||
}
|
||||
|
||||
void matchTwist(const nav_2d_msgs::msg::Twist2D & a, const nav_2d_msgs::msg::Twist2D & b)
|
||||
{
|
||||
EXPECT_DOUBLE_EQ(a.x, b.x);
|
||||
EXPECT_DOUBLE_EQ(a.y, b.y);
|
||||
EXPECT_DOUBLE_EQ(a.theta, b.theta);
|
||||
}
|
||||
|
||||
void matchTwist(
|
||||
const nav_2d_msgs::msg::Twist2D & a, const double x, const double y,
|
||||
const double theta)
|
||||
{
|
||||
EXPECT_DOUBLE_EQ(a.x, x);
|
||||
EXPECT_DOUBLE_EQ(a.y, y);
|
||||
EXPECT_DOUBLE_EQ(a.theta, theta);
|
||||
}
|
||||
|
||||
const double DEFAULT_SIM_TIME = 1.7;
|
||||
|
||||
TEST(TrajectoryGenerator, basic)
|
||||
{
|
||||
auto nh = makeTestNode("basic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
|
||||
matchTwist(res.velocity, forward);
|
||||
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME);
|
||||
int n = res.poses.size();
|
||||
EXPECT_EQ(n, 4);
|
||||
ASSERT_GT(n, 0);
|
||||
|
||||
matchPose(res.poses[0], origin);
|
||||
matchPose(res.poses[n - 1], DEFAULT_SIM_TIME * forward.x, 0, 0);
|
||||
}
|
||||
|
||||
TEST(TrajectoryGenerator, basic_no_last_point)
|
||||
{
|
||||
auto nh = makeTestNode(
|
||||
"basic_no_last_point", {
|
||||
rclcpp::Parameter("dwb.include_last_point", false),
|
||||
rclcpp::Parameter("dwb.linear_granularity", 0.5)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
|
||||
matchTwist(res.velocity, forward);
|
||||
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME / 2);
|
||||
int n = res.poses.size();
|
||||
EXPECT_EQ(n, 3);
|
||||
ASSERT_GT(n, 0);
|
||||
|
||||
matchPose(res.poses[0], origin);
|
||||
matchPose(res.poses[n - 2], 0.255, 0, 0);
|
||||
}
|
||||
|
||||
TEST(TrajectoryGenerator, too_slow)
|
||||
{
|
||||
auto nh = makeTestNode("too_slow", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
nav_2d_msgs::msg::Twist2D cmd;
|
||||
cmd.x = 0.2;
|
||||
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
|
||||
matchTwist(res.velocity, cmd);
|
||||
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME);
|
||||
int n = res.poses.size();
|
||||
EXPECT_EQ(n, 3);
|
||||
ASSERT_GT(n, 0);
|
||||
|
||||
matchPose(res.poses[0], origin);
|
||||
}
|
||||
|
||||
TEST(TrajectoryGenerator, holonomic)
|
||||
{
|
||||
auto nh = makeTestNode("holonomic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
nav_2d_msgs::msg::Twist2D cmd;
|
||||
cmd.x = 0.3;
|
||||
cmd.y = 0.2;
|
||||
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
|
||||
matchTwist(res.velocity, cmd);
|
||||
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME);
|
||||
int n = res.poses.size();
|
||||
EXPECT_EQ(n, 4);
|
||||
ASSERT_GT(n, 0);
|
||||
|
||||
matchPose(res.poses[0], origin);
|
||||
matchPose(res.poses[n - 1], cmd.x * DEFAULT_SIM_TIME, cmd.y * DEFAULT_SIM_TIME, 0);
|
||||
}
|
||||
|
||||
TEST(TrajectoryGenerator, twisty)
|
||||
{
|
||||
auto nh = makeTestNode(
|
||||
"twisty", {
|
||||
rclcpp::Parameter("dwb.linear_granularity", 0.5),
|
||||
rclcpp::Parameter("dwb.angular_granularity", 0.025)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
nav_2d_msgs::msg::Twist2D cmd;
|
||||
cmd.x = 0.3;
|
||||
cmd.y = -0.2;
|
||||
cmd.theta = 0.111;
|
||||
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
|
||||
matchTwist(res.velocity, cmd);
|
||||
EXPECT_NEAR(durationToSec(res.time_offsets.back()), DEFAULT_SIM_TIME, 1.0E-5);
|
||||
int n = res.poses.size();
|
||||
EXPECT_EQ(n, 10);
|
||||
ASSERT_GT(n, 0);
|
||||
|
||||
matchPose(res.poses[0], origin);
|
||||
matchPose(
|
||||
res.poses[n - 1], 0.5355173615993063, -0.29635287789821596,
|
||||
cmd.theta * DEFAULT_SIM_TIME);
|
||||
}
|
||||
|
||||
TEST(TrajectoryGenerator, sim_time)
|
||||
{
|
||||
const double sim_time = 2.5;
|
||||
auto nh = makeTestNode(
|
||||
"sim_time", {
|
||||
rclcpp::Parameter("dwb.sim_time", sim_time),
|
||||
rclcpp::Parameter("dwb.linear_granularity", 0.5)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
|
||||
matchTwist(res.velocity, forward);
|
||||
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), sim_time);
|
||||
int n = res.poses.size();
|
||||
EXPECT_EQ(n, 4);
|
||||
ASSERT_GT(n, 0);
|
||||
|
||||
matchPose(res.poses[0], origin);
|
||||
matchPose(res.poses[n - 2], sim_time * forward.x, 0, 0);
|
||||
}
|
||||
|
||||
TEST(TrajectoryGenerator, accel)
|
||||
{
|
||||
auto nh = makeTestNode(
|
||||
"accel", {
|
||||
rclcpp::Parameter("dwb.sim_time", 5.0),
|
||||
rclcpp::Parameter("dwb.discretize_by_time", true),
|
||||
rclcpp::Parameter("dwb.time_granularity", 1.0),
|
||||
rclcpp::Parameter("dwb.acc_lim_x", 0.1),
|
||||
rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
|
||||
StandardTrajectoryGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
|
||||
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
|
||||
matchTwist(res.velocity, forward);
|
||||
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), 5.0);
|
||||
ASSERT_EQ(res.poses.size(), 7u);
|
||||
matchPose(res.poses[0], origin);
|
||||
matchPose(res.poses[1], 0.1, 0, 0);
|
||||
matchPose(res.poses[2], 0.3, 0, 0);
|
||||
matchPose(res.poses[3], 0.6, 0, 0);
|
||||
matchPose(res.poses[4], 0.9, 0, 0);
|
||||
matchPose(res.poses[5], 1.2, 0, 0);
|
||||
}
|
||||
|
||||
TEST(TrajectoryGenerator, dwa)
|
||||
{
|
||||
auto nh = makeTestNode(
|
||||
"dwa", {
|
||||
rclcpp::Parameter("dwb.sim_period", 1.0),
|
||||
rclcpp::Parameter("dwb.sim_time", 5.0),
|
||||
rclcpp::Parameter("dwb.discretize_by_time", true),
|
||||
rclcpp::Parameter("dwb.time_granularity", 1.0),
|
||||
rclcpp::Parameter("dwb.acc_lim_x", 0.1),
|
||||
rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
|
||||
dwb_plugins::LimitedAccelGenerator gen;
|
||||
gen.initialize(nh, "dwb");
|
||||
|
||||
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
|
||||
matchTwist(res.velocity, forward);
|
||||
EXPECT_DOUBLE_EQ(durationToSec(res.time_offsets.back()), 5.0);
|
||||
ASSERT_EQ(res.poses.size(), 7u);
|
||||
matchPose(res.poses[0], origin);
|
||||
matchPose(res.poses[1], 0.3, 0, 0);
|
||||
matchPose(res.poses[2], 0.6, 0, 0);
|
||||
matchPose(res.poses[3], 0.9, 0, 0);
|
||||
matchPose(res.poses[4], 1.2, 0, 0);
|
||||
matchPose(res.poses[5], 1.5, 0, 0);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
forward.x = 0.3;
|
||||
rclcpp::init(0, nullptr);
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
int ret = RUN_ALL_TESTS();
|
||||
rclcpp::shutdown();
|
||||
return ret;
|
||||
}
|
||||
@@ -0,0 +1,143 @@
|
||||
/*
|
||||
* 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 "gtest/gtest.h"
|
||||
#include "dwb_plugins/one_d_velocity_iterator.hpp"
|
||||
|
||||
using dwb_plugins::OneDVelocityIterator;
|
||||
|
||||
const double EPSILON = 1e-3;
|
||||
|
||||
TEST(VelocityIterator, basics)
|
||||
{
|
||||
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 2);
|
||||
EXPECT_FALSE(it.isFinished());
|
||||
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
|
||||
EXPECT_FALSE(it.isFinished());
|
||||
++it;
|
||||
EXPECT_FALSE(it.isFinished());
|
||||
EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
|
||||
EXPECT_FALSE(it.isFinished());
|
||||
++it;
|
||||
EXPECT_TRUE(it.isFinished());
|
||||
it.reset();
|
||||
EXPECT_FALSE(it.isFinished());
|
||||
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, limits)
|
||||
{
|
||||
OneDVelocityIterator it(2.0, 1.5, 2.5, 1.0, -1.0, 1.0, 2);
|
||||
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, acceleration)
|
||||
{
|
||||
OneDVelocityIterator it(2.0, 0.0, 5.0, 0.5, -0.5, 1.0, 2);
|
||||
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
|
||||
}
|
||||
|
||||
|
||||
TEST(VelocityIterator, time)
|
||||
{
|
||||
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 0.5, 2);
|
||||
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, samples)
|
||||
{
|
||||
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 3);
|
||||
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_TRUE(it.isFinished());
|
||||
}
|
||||
|
||||
|
||||
TEST(VelocityIterator, samples2)
|
||||
{
|
||||
OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 5);
|
||||
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_TRUE(it.isFinished());
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, around_zero)
|
||||
{
|
||||
OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 2);
|
||||
EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
|
||||
++it;
|
||||
}
|
||||
|
||||
TEST(VelocityIterator, around_zero2)
|
||||
{
|
||||
OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 4);
|
||||
EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), -0.3333, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 0.3333, EPSILON);
|
||||
++it;
|
||||
EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
|
||||
++it;
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user