Skip to content

Commit

Permalink
Added thread synchronization to KinematicParameters (#1459) (#1621)
Browse files Browse the repository at this point in the history 
* Added thread synchronization to KinematicParameters (#1459)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Doxygen fix (#1459)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
  • Loading branch information
@gimait
gimait authored Apr 2, 2020
1 parent 826a4d2 commit db4193f
Show file tree
Hide file tree
Showing 7 changed files with 116 additions and 73 deletions.
35 changes: 25 additions & 10 deletions nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,14 +45,12 @@ namespace dwb_plugins
{

/**
* @class KinematicParameters
* @brief A class containing one representation of the robot's kinematics
* @struct KinematicParameters
* @brief A struct containing one representation of the robot's kinematics
*/
class KinematicParameters
struct KinematicParameters
{
public:
KinematicParameters();
void initialize(const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name);
friend class KinematicsHandler;

inline double getMinX() {return min_vel_x_;}
inline double getMaxX() {return max_vel_x_;}
Expand Down Expand Up @@ -90,8 +88,6 @@ class KinematicParameters
*/
bool isValidSpeed(double x, double y, double theta);

using Ptr = std::shared_ptr<KinematicParameters>;

protected:
// For parameter descriptions, see cfg/KinematicParams.cfg
double min_vel_x_{0};
Expand All @@ -112,14 +108,33 @@ class KinematicParameters
// 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();}

bool isValidSpeed(double x, double y, double theta);

using Ptr = std::shared_ptr<KinematicsHandler>;

void reconfigureCB();
protected:
std::atomic<KinematicParameters *> kinematics_;

// Subscription for parameter change
rclcpp::AsyncParametersClient::SharedPtr parameters_client_;
rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub_;
void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);

void update_kinematics(KinematicParameters kinematics);
std::string plugin_name_;
};

Expand Down
2 changes: 1 addition & 1 deletion nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator
*/
virtual std::vector<double> getTimeSteps(const nav_2d_msgs::msg::Twist2D & cmd_vel);

KinematicParameters::Ptr kinematics_;
KinematicsHandler::Ptr kinematics_handler_;
std::shared_ptr<VelocityIterator> velocity_iterator_;

double sim_time_;
Expand Down
2 changes: 1 addition & 1 deletion nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ class VelocityIterator
virtual ~VelocityIterator() {}
virtual void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
KinematicParameters::Ptr kinematics,
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;
Expand Down
6 changes: 3 additions & 3 deletions nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,10 @@ class XYThetaIterator : public VelocityIterator
{
public:
XYThetaIterator()
: kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {}
: kinematics_handler_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {}
void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
KinematicParameters::Ptr kinematics,
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;
Expand All @@ -61,7 +61,7 @@ class XYThetaIterator : public VelocityIterator
virtual bool isValidVelocity();
void iterateToValidVelocity();
int vx_samples_, vy_samples_, vtheta_samples_;
KinematicParameters::Ptr kinematics_;
KinematicsHandler::Ptr kinematics_handler_;

std::shared_ptr<OneDVelocityIterator> x_it_, y_it_, th_it_;
};
Expand Down
108 changes: 67 additions & 41 deletions nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,11 +52,27 @@ using std::placeholders::_1;
namespace dwb_plugins
{

KinematicParameters::KinematicParameters()
bool KinematicParameters::isValidSpeed(double x, double y, double theta)
{
double vmag_sq = x * x + y * y;
if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON) {return false;}
if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ &&
min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_) {return false;}
if (vmag_sq == 0.0 && theta == 0.0) {return false;}
return true;
}

KinematicsHandler::KinematicsHandler()
{
kinematics_.store(new KinematicParameters);
}

KinematicsHandler::~KinematicsHandler()
{
delete kinematics_.load();
}

void KinematicParameters::initialize(
void KinematicsHandler::initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name)
{
Expand Down Expand Up @@ -94,20 +110,22 @@ void KinematicParameters::initialize(
nh, plugin_name + ".decel_lim_theta",
rclcpp::ParameterValue(0.0));

nh->get_parameter(plugin_name + ".min_vel_x", min_vel_x_);
nh->get_parameter(plugin_name + ".min_vel_y", min_vel_y_);
nh->get_parameter(plugin_name + ".max_vel_x", max_vel_x_);
nh->get_parameter(plugin_name + ".max_vel_y", max_vel_y_);
nh->get_parameter(plugin_name + ".max_vel_theta", max_vel_theta_);
nh->get_parameter(plugin_name + ".min_speed_xy", min_speed_xy_);
nh->get_parameter(plugin_name + ".max_speed_xy", max_speed_xy_);
nh->get_parameter(plugin_name + ".min_speed_theta", min_speed_theta_);
nh->get_parameter(plugin_name + ".acc_lim_x", acc_lim_x_);
nh->get_parameter(plugin_name + ".acc_lim_y", acc_lim_y_);
nh->get_parameter(plugin_name + ".acc_lim_theta", acc_lim_theta_);
nh->get_parameter(plugin_name + ".decel_lim_x", decel_lim_x_);
nh->get_parameter(plugin_name + ".decel_lim_y", decel_lim_y_);
nh->get_parameter(plugin_name + ".decel_lim_theta", decel_lim_theta_);
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_);

// Setup callback for changes to parameters.
parameters_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
Expand All @@ -117,63 +135,71 @@ void KinematicParameters::initialize(
nh->get_node_services_interface());

parameter_event_sub_ = parameters_client_->on_parameter_event(
std::bind(&KinematicParameters::on_parameter_event_callback, this, _1));
std::bind(&KinematicsHandler::on_parameter_event_callback, this, _1));

min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_;
max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_;
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);
}

bool KinematicParameters::isValidSpeed(double x, double y, double theta)
bool KinematicsHandler::isValidSpeed(double x, double y, double theta)
{
double vmag_sq = x * x + y * y;
if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON) {return false;}
if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ &&
min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_) {return false;}
if (vmag_sq == 0.0 && theta == 0.0) {return false;}
return true;
return kinematics_.load()->isValidSpeed(x, y, theta);
}

void
KinematicParameters::on_parameter_event_callback(
KinematicsHandler::on_parameter_event_callback(
const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
{
KinematicParameters kinematics(*kinematics_.load());

for (auto & changed_parameter : event->changed_parameters) {
const auto & type = changed_parameter.value.type;
const auto & name = changed_parameter.name;
const auto & value = changed_parameter.value;

if (type == ParameterType::PARAMETER_DOUBLE) {
if (name == plugin_name_ + ".min_vel_x") {
min_vel_x_ = value.double_value;
kinematics.min_vel_x_ = value.double_value;
} else if (name == plugin_name_ + ".min_vel_y") {
min_vel_y_ = value.double_value;
kinematics.min_vel_y_ = value.double_value;
} else if (name == plugin_name_ + ".max_vel_x") {
max_vel_x_ = value.double_value;
kinematics.max_vel_x_ = value.double_value;
} else if (name == plugin_name_ + ".max_vel_y") {
max_vel_y_ = value.double_value;
kinematics.max_vel_y_ = value.double_value;
} else if (name == plugin_name_ + ".max_vel_theta") {
max_vel_theta_ = value.double_value;
kinematics.max_vel_theta_ = value.double_value;
} else if (name == plugin_name_ + ".min_speed_xy") {
min_speed_xy_ = value.double_value;
kinematics.min_speed_xy_ = value.double_value;
kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
} else if (name == plugin_name_ + ".max_speed_xy") {
max_speed_xy_ = value.double_value;
kinematics.max_speed_xy_ = value.double_value;
} else if (name == plugin_name_ + ".min_speed_theta") {
min_speed_theta_ = value.double_value;
kinematics.min_speed_theta_ = value.double_value;
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
} else if (name == plugin_name_ + ".acc_lim_x") {
acc_lim_x_ = value.double_value;
kinematics.acc_lim_x_ = value.double_value;
} else if (name == plugin_name_ + ".acc_lim_y") {
acc_lim_y_ = value.double_value;
kinematics.acc_lim_y_ = value.double_value;
} else if (name == plugin_name_ + ".acc_lim_theta") {
acc_lim_theta_ = value.double_value;
kinematics.acc_lim_theta_ = value.double_value;
} else if (name == plugin_name_ + ".decel_lim_x") {
decel_lim_x_ = value.double_value;
kinematics.decel_lim_x_ = value.double_value;
} else if (name == plugin_name_ + ".decel_lim_y") {
decel_lim_y_ = value.double_value;
kinematics.decel_lim_y_ = value.double_value;
} else if (name == plugin_name_ + ".decel_lim_theta") {
decel_lim_theta_ = value.double_value;
kinematics.decel_lim_theta_ = value.double_value;
}
}
}
update_kinematics(kinematics);
}

void KinematicsHandler::update_kinematics(KinematicParameters kinematics)
{
delete kinematics_.load();
kinematics_.store(new KinematicParameters(kinematics));
}

} // namespace dwb_plugins
17 changes: 9 additions & 8 deletions nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,8 @@ void StandardTrajectoryGenerator::initialize(
const std::string & plugin_name)
{
plugin_name_ = plugin_name;
kinematics_ = std::make_shared<KinematicParameters>();
kinematics_->initialize(nh, plugin_name_);
kinematics_handler_ = std::make_shared<KinematicsHandler>();
kinematics_handler_->initialize(nh, plugin_name_);
initializeIterator(nh);

nav2_util::declare_parameter_if_not_declared(
Expand Down Expand Up @@ -97,7 +97,7 @@ void StandardTrajectoryGenerator::initializeIterator(
const nav2_util::LifecycleNode::SharedPtr & nh)
{
velocity_iterator_ = std::make_shared<XYThetaIterator>();
velocity_iterator_->initialize(nh, kinematics_, plugin_name_);
velocity_iterator_->initialize(nh, kinematics_handler_, plugin_name_);
}

void StandardTrajectoryGenerator::startNewIteration(
Expand Down Expand Up @@ -185,16 +185,17 @@ 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);
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);
start_vel.y, kinematics.getAccY(),
kinematics.getDecelY(), dt, cmd_vel.y);
new_vel.theta = projectVelocity(
start_vel.theta,
kinematics_->getAccTheta(), kinematics_->getDecelTheta(),
kinematics.getAccTheta(), kinematics.getDecelTheta(),
dt, cmd_vel.theta);
return new_vel;
}
Expand Down
19 changes: 10 additions & 9 deletions nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,10 +42,10 @@ namespace dwb_plugins
{
void XYThetaIterator::initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
KinematicParameters::Ptr kinematics,
KinematicsHandler::Ptr kinematics,
const std::string & plugin_name)
{
kinematics_ = kinematics;
kinematics_handler_ = kinematics;

nav2_util::declare_parameter_if_not_declared(
nh,
Expand All @@ -66,18 +66,19 @@ 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_);
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_);
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(),
kinematics.getMinTheta(), kinematics.getMaxTheta(),
kinematics.getAccTheta(), kinematics.getDecelTheta(),
dt, vtheta_samples_);
if (!isValidVelocity()) {
iterateToValidVelocity();
Expand All @@ -86,7 +87,7 @@ void XYThetaIterator::startNewIteration(

bool XYThetaIterator::isValidVelocity()
{
return kinematics_->isValidSpeed(
return kinematics_handler_->isValidSpeed(
x_it_->getVelocity(), y_it_->getVelocity(),
th_it_->getVelocity());
}
Expand Down

0 comments on commit db4193f

Please sign in to comment.