Skip to content

Commit

Permalink
hector_quadrotor_controllers/hector_quadrotor_interface: shutdown sub…
Browse files Browse the repository at this point in the history
…scribers explicitly

This patch seems to fix "boost: mutex lock failed in pthread_mutex_lock: Invalid argument" exceptions during shutdown
if the callbacks are called after the mutex has already been destroyed.

Probably ordering the boost::mutex members before the subscribers and rely on proper destruction order would also have solved the issue.
  • Loading branch information
meyerj committed Mar 5, 2017
1 parent 58f4eb3 commit 1b268f5
Show file tree
Hide file tree
Showing 4 changed files with 33 additions and 0 deletions.
2 changes: 2 additions & 0 deletions hector_quadrotor_controllers/src/attitude_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,8 @@ class AttitudeController : public controller_interface::Controller<hector_quadro

virtual ~AttitudeController()
{
attitude_subscriber_helper_.reset();
estop_sub_.shutdown();
}

virtual bool init(hector_quadrotor_interface::QuadrotorInterface *interface, ros::NodeHandle &root_nh,
Expand Down
2 changes: 2 additions & 0 deletions hector_quadrotor_controllers/src/position_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,8 @@ class PositionController : public controller_interface::Controller<QuadrotorInte

virtual ~PositionController()
{
pose_subscriber_.shutdown();
twist_limit_subscriber_.shutdown();
}

virtual bool init(QuadrotorInterface *interface,
Expand Down
2 changes: 2 additions & 0 deletions hector_quadrotor_controllers/src/velocity_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,8 @@ class VelocityController : public controller_interface::Controller<hector_quadro

virtual ~VelocityController()
{
twist_subscriber_.shutdown();
cmd_vel_subscriber_.shutdown();
}

virtual bool init(hector_quadrotor_interface::QuadrotorInterface *interface, ros::NodeHandle &root_nh,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,11 @@ class ImuSubscriberHelper
imu_sub_ = nh.subscribe<sensor_msgs::Imu>(topic, 1, boost::bind(&ImuSubscriberHelper::imuCallback, this, _1));
}

~ImuSubscriberHelper()
{
imu_sub_.shutdown();
}

private:
sensor_msgs::Imu &imu_;
ros::Subscriber imu_sub_;
Expand All @@ -85,6 +90,11 @@ class OdomSubscriberHelper
boost::bind(&OdomSubscriberHelper::odomCallback, this, _1));
}

~OdomSubscriberHelper()
{
odom_sub_.shutdown();
}

private:
ros::Subscriber odom_sub_;

Expand Down Expand Up @@ -140,6 +150,11 @@ class PoseSubscriberHelper
_1));
}

~PoseSubscriberHelper()
{
pose_sub_.shutdown();
}

geometry_msgs::PoseStampedConstPtr get() const { return pose_; }

private:
Expand Down Expand Up @@ -246,6 +261,11 @@ class StateSubscriberHelper
this, _1));
}

~StateSubscriberHelper()
{
tf_sub_.shutdown();
}

bool isAvailable(){ return available_; }

private:
Expand Down Expand Up @@ -323,6 +343,13 @@ class AttitudeSubscriberHelper
&AttitudeSubscriberHelper::thrustCommandCb, this, _1));
}

~AttitudeSubscriberHelper()
{
attitude_subscriber_.shutdown();
yawrate_subscriber_.shutdown();
thrust_subscriber_.shutdown();
}

private:
ros::Subscriber attitude_subscriber_, yawrate_subscriber_, thrust_subscriber_;
boost::mutex &command_mutex_;
Expand Down

0 comments on commit 1b268f5

Please sign in to comment.