Skip to content
New issue

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

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

Already on GitHub? Sign in to your account

Added thread synchronization to KinematicParameters (#1459) #1621

Merged
merged 2 commits into from
Apr 2, 2020
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -48,12 +48,8 @@ namespace dwb_plugins
* @class KinematicParameters
Copy link
Member

Choose a reason for hiding this comment

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

Update this doxy

* @brief A class 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);

inline double getMinX() {return min_vel_x_;}
inline double getMaxX() {return max_vel_x_;}
inline double getAccX() {return acc_lim_x_;}
Expand Down Expand Up @@ -90,8 +86,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 @@ -113,13 +107,34 @@ class KinematicParameters
double min_speed_xy_sq_{0};
double max_speed_xy_sq_{0};

void reconfigureCB();
friend class KinematicsHandler;
Copy link
Member

Choose a reason for hiding this comment

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

Can you put this at the top of the struct? It'll make that more clear to passive readers

Copy link
Contributor Author

Choose a reason for hiding this comment

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

For sure, I'm on it.

};

/**
* @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>;

protected:
std::atomic<KinematicParameters *> kinematics_;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved

// 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
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
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
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
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
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
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