Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Jump to bottom

fix threading bug in point head controller #62

Merged
merged 1 commit into from Aug 24, 2020
Merged

fix threading bug in point head controller #62

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
7 changes: 6 additions & 1 deletion robot_controllers/include/robot_controllers/point_head.h
View file
Open in desktop
Expand Up @@ -132,6 +132,9 @@ class PointHeadController : public robot_controllers_interface::Controller
const std::shared_ptr<PointHeadGoal> goal_handle);
void handle_accepted(const std::shared_ptr<PointHeadGoal> goal_handle);

// Execute the goal
void execute(const std::shared_ptr<PointHeadGoal> goal_handle);

// Handles to node, manager
rclcpp::Node::SharedPtr node_;
robot_controllers_interface::ControllerManagerPtr manager_;
Expand All @@ -148,7 +151,6 @@ class PointHeadController : public robot_controllers_interface::Controller

// Trajectory generation
std::shared_ptr<TrajectorySampler> sampler_;
std::mutex sampler_mutex_;

/*
* In certain cases, we want to start a trajectory at our last sample,
Expand All @@ -161,6 +163,9 @@ class PointHeadController : public robot_controllers_interface::Controller
// RCLCPP action server
rclcpp_action::Server<PointHeadAction>::SharedPtr server_;
std::shared_ptr<PointHeadGoal> active_goal_;
// This guards both the active_goal_ and the sampler_
// Is recursive so we can call stop whenever we want
std::recursive_mutex active_goal_mutex_;

KDL::Tree kdl_tree_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
Expand Down
37 changes: 25 additions & 12 deletions robot_controllers/src/point_head.cpp
View file
Open in desktop
Expand Up @@ -73,12 +73,14 @@ int PointHeadController::init(const std::string& name,
return -1;
}

// Lock before resetting things
std::lock_guard<std::recursive_mutex> lock(active_goal_mutex_);

Controller::init(name, node, manager);
node_ = node;
manager_ = manager;

// No initial sampler
std::lock_guard<std::mutex> lock(sampler_mutex_);
sampler_.reset();

// Get parameters
Expand Down Expand Up @@ -185,6 +187,7 @@ bool PointHeadController::stop(bool force)
if (!server_)
return true;

std::lock_guard<std::recursive_mutex> lock(active_goal_mutex_);
if (active_goal_)
{
if (force)
Expand Down Expand Up @@ -216,25 +219,30 @@ void PointHeadController::update(const rclcpp::Time& now, const rclcpp::Duration
if (!server_)
return;

// Prevent our goal/sampler from changing
std::lock_guard<std::recursive_mutex> lock(active_goal_mutex_);

// We have a trajectory to execute?
if (active_goal_ && sampler_)
{
std::lock_guard<std::mutex> lock(sampler_mutex_);

// Interpolate trajectory
TrajectoryPoint p = sampler_->sample(to_sec(now));
last_sample_ = p;

// Are we done?
if (to_sec(now) > sampler_->end_time())
{
// Stop this controller if desired (and not preempted)
if (stop_with_action_)
manager_->requestStop(getName());

// Mark succeeded first (so we don't abort in requestStop)
auto result = std::make_shared<PointHeadAction::Result>();
active_goal_->succeed(result);
active_goal_.reset();

// Stop this controller if desired
if (stop_with_action_)
{
manager_->requestStop(getName());
}

RCLCPP_DEBUG(rclcpp::get_logger(getName()),
"PointHead goal succeeded");
}
Expand Down Expand Up @@ -275,6 +283,7 @@ rclcpp_action::CancelResponse PointHeadController::handle_cancel(
const std::shared_ptr<PointHeadGoal> goal_handle)
{
// Always accept
std::lock_guard<std::recursive_mutex> lock(active_goal_mutex_);
if (active_goal_ && active_goal_->get_goal_id() == goal_handle->get_goal_id())
{
RCLCPP_INFO(rclcpp::get_logger(getName()),
Expand All @@ -289,6 +298,13 @@ rclcpp_action::CancelResponse PointHeadController::handle_cancel(

void PointHeadController::handle_accepted(const std::shared_ptr<PointHeadGoal> goal_handle)
{
// This needs to return ASAP, so detach the actual work
std::thread{std::bind(&PointHeadController::execute, this, _1), goal_handle}.detach();
}

void PointHeadController::execute(const std::shared_ptr<PointHeadGoal> goal_handle)
{
std::lock_guard<std::recursive_mutex> lock(active_goal_mutex_);
auto result = std::make_shared<PointHeadAction::Result>();

bool preempted = false;
Expand Down Expand Up @@ -375,11 +391,8 @@ void PointHeadController::handle_accepted(const std::shared_ptr<PointHeadGoal> g
t.points[1].time = t.points[0].time + fmax(fmax(pan_transit, tilt_transit),
msg_to_sec(goal->min_duration));

{
std::lock_guard<std::mutex> lock(sampler_mutex_);
sampler_.reset(new SplineTrajectorySampler(t));
active_goal_ = goal_handle;
}
sampler_.reset(new SplineTrajectorySampler(t));
active_goal_ = goal_handle;

if (manager_->requestStart(getName()) != 0)
{
Expand Down