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

safety_limiter: fix diagnostics warning condition #374

Merged
merged 6 commits into from
Sep 9, 2019
Merged

safety_limiter: fix diagnostics warning condition #374

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
79bcb4b
safety_limiter: fix diagnostics warning condition
at-wat Sep 9, 2019
05903c0
Test diag output
at-wat Sep 9, 2019
2c97074
Fix diag output
at-wat Sep 9, 2019
eaa1f7a
Fix diag output for timeout conditions
at-wat Sep 9, 2019
f0fb0ae
Immediately update diag on timeout
at-wat Sep 9, 2019
cff08d0
Revert unused lambda capture
at-wat Sep 9, 2019
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
23 changes: 18 additions & 5 deletions safety_limiter/src/safety_limiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -283,8 +283,11 @@ class SafetyLimiterNode
{
ROS_WARN_THROTTLE(1.0, "safety_limiter: Watchdog timed-out");
watchdog_stop_ = true;
r_lim_ = 0;
geometry_msgs::Twist cmd_vel;
pub_twist_.publish(cmd_vel);

diag_updater_.force_update();
}
void cbPredictTimer(const ros::TimerEvent& event)
{
Expand All @@ -300,6 +303,10 @@ class SafetyLimiterNode
pub_twist_.publish(cmd_vel);

cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
has_cloud_ = false;
r_lim_ = 0;

diag_updater_.force_update();
return;
}

Expand Down Expand Up @@ -463,9 +470,9 @@ class SafetyLimiterNode
std::sqrt(std::pow(acc_dtsq[1], 2) + 2 * acc_dtsq[1] * std::abs(yaw_col)));

float d_r =
(std::sqrt(std::abs(2 * acc_[0] * d_col)) + EPSILON) / std::abs(twist_.linear.x);
std::sqrt(std::abs(2 * acc_[0] * d_col)) / std::abs(twist_.linear.x);
float yaw_r =
(std::sqrt(std::abs(2 * acc_[1] * yaw_col)) + EPSILON) / std::abs(twist_.angular.z);
std::sqrt(std::abs(2 * acc_[1] * yaw_col)) / std::abs(twist_.angular.z);
if (!std::isfinite(d_r))
d_r = 1.0;
if (!std::isfinite(yaw_r))
Expand Down Expand Up @@ -610,7 +617,7 @@ class SafetyLimiterNode
{
pub_twist_.publish(twist_);
}
else if (ros::Time::now() - last_cloud_stamp_ > ros::Duration(timeout_) || watchdog_stop_)
else if (!has_cloud_ || watchdog_stop_)
{
geometry_msgs::Twist cmd_vel;
pub_twist_.publish(cmd_vel);
Expand Down Expand Up @@ -661,11 +668,15 @@ class SafetyLimiterNode

void diagnoseCollision(diagnostic_updater::DiagnosticStatusWrapper& stat)
{
if (r_lim_ == 1.0)
if (!has_cloud_ || watchdog_stop_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Stopped due to data timeout.");
}
else if (r_lim_ == 1.0)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
}
else if (r_lim_ == 0.0)
else if (r_lim_ < EPSILON)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN,
(has_collision_at_now_) ?
Expand All @@ -680,6 +691,8 @@ class SafetyLimiterNode
"Reducing velocity to avoid collision.");
}
stat.addf("Velocity Limit Ratio", "%.2f", r_lim_);
stat.add("Pointcloud Availability", has_cloud_ ? "true" : "false");
stat.add("Watchdog Timeout", watchdog_stop_ ? "true" : "false");
}
};

Expand Down
19 changes: 19 additions & 0 deletions safety_limiter/test/include/test_safety_limiter_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@
#include <string>

#include <ros/ros.h>

#include <diagnostic_msgs/DiagnosticArray.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
Expand Down Expand Up @@ -71,14 +73,23 @@ class SafetyLimiterTest : public ::testing::Test
ros::Publisher pub_cmd_vel_;
ros::Publisher pub_cloud_;
ros::Publisher pub_watchdog_;
ros::Subscriber sub_diag_;

inline void cbDiag(const diagnostic_msgs::DiagnosticArray::ConstPtr& msg)
{
diag_ = msg;
}

public:
diagnostic_msgs::DiagnosticArray::ConstPtr diag_;

inline SafetyLimiterTest()
: nh_()
{
pub_cmd_vel_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_in", 1);
pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("cloud", 1);
pub_watchdog_ = nh_.advertise<std_msgs::Empty>("watchdog_reset", 1);
sub_diag_ = nh_.subscribe("diagnostics", 1, &SafetyLimiterTest::cbDiag, this);
}
inline void publishWatchdogReset()
{
Expand Down Expand Up @@ -107,6 +118,14 @@ class SafetyLimiterTest : public ::testing::Test
cmd_vel_out.angular.z = ang;
pub_cmd_vel_.publish(cmd_vel_out);
}
inline bool hasDiag() const
{
if (!diag_)
return false;
if (diag_->status.size() == 0)
return false;
return true;
}
};

#endif // TEST_SAFETY_LIMITER_BASE_H
87 changes: 75 additions & 12 deletions safety_limiter/test/src/test_safety_limiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@
*/

#include <ros/ros.h>

#include <diagnostic_msgs/DiagnosticStatus.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>

Expand All @@ -54,6 +56,9 @@ TEST_F(SafetyLimiterTest, Timeouts)
{
for (int with_watchdog_reset = 0; with_watchdog_reset < 2; ++with_watchdog_reset)
{
const std::string test_condition =
"with_watchdog_reset: " + std::to_string(with_watchdog_reset) +
", with_cloud: " + std::to_string(with_cloud);
ros::Duration(0.3).sleep();

cmd_vel.reset();
Expand Down Expand Up @@ -84,24 +89,38 @@ TEST_F(SafetyLimiterTest, Timeouts)
{
if (with_watchdog_reset > 0 && with_cloud > 1)
{
ASSERT_EQ(cmd_vel->linear.x, vel);
ASSERT_EQ(cmd_vel->linear.y, 0.0);
ASSERT_EQ(cmd_vel->linear.z, 0.0);
ASSERT_EQ(cmd_vel->angular.x, 0.0);
ASSERT_EQ(cmd_vel->angular.y, 0.0);
ASSERT_EQ(cmd_vel->angular.z, ang_vel);
ASSERT_EQ(cmd_vel->linear.x, vel) << test_condition;
ASSERT_EQ(cmd_vel->linear.y, 0.0) << test_condition;
ASSERT_EQ(cmd_vel->linear.z, 0.0) << test_condition;
ASSERT_EQ(cmd_vel->angular.x, 0.0) << test_condition;
ASSERT_EQ(cmd_vel->angular.y, 0.0) << test_condition;
ASSERT_EQ(cmd_vel->angular.z, ang_vel) << test_condition;
}
else
{
ASSERT_EQ(cmd_vel->linear.x, 0.0);
ASSERT_EQ(cmd_vel->linear.y, 0.0);
ASSERT_EQ(cmd_vel->linear.z, 0.0);
ASSERT_EQ(cmd_vel->angular.x, 0.0);
ASSERT_EQ(cmd_vel->angular.y, 0.0);
ASSERT_EQ(cmd_vel->angular.z, 0.0);
ASSERT_EQ(cmd_vel->linear.x, 0.0) << test_condition;
ASSERT_EQ(cmd_vel->linear.y, 0.0) << test_condition;
ASSERT_EQ(cmd_vel->linear.z, 0.0) << test_condition;
ASSERT_EQ(cmd_vel->angular.x, 0.0) << test_condition;
ASSERT_EQ(cmd_vel->angular.y, 0.0) << test_condition;
ASSERT_EQ(cmd_vel->angular.z, 0.0) << test_condition;
}
}
}
if (with_watchdog_reset > 0 && with_cloud > 1)
{
ASSERT_TRUE(hasDiag()) << test_condition;
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
<< test_condition << ", "
<< "message: " << diag_->status[0].message;
}
else
{
ASSERT_TRUE(hasDiag()) << test_condition;
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, diag_->status[0].level)
<< test_condition << ", "
<< "message: " << diag_->status[0].message;
}
}
}
}
Expand Down Expand Up @@ -202,6 +221,10 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinear)
wait.sleep();
ros::spinOnce();
}
ASSERT_TRUE(hasDiag());
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
<< "message: " << diag_->status[0].message;

ASSERT_TRUE(received);
sub_cmd_vel.shutdown();
}
Expand Down Expand Up @@ -251,6 +274,10 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearBackward)
wait.sleep();
ros::spinOnce();
}
ASSERT_TRUE(hasDiag());
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
<< "message: " << diag_->status[0].message;

ASSERT_TRUE(received);
sub_cmd_vel.shutdown();
}
Expand Down Expand Up @@ -305,6 +332,18 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearEscape)
wait.sleep();
ros::spinOnce();
}
if (vel < 0)
{
ASSERT_TRUE(hasDiag());
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
<< "message: " << diag_->status[0].message;
}
else
{
ASSERT_TRUE(hasDiag());
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::WARN, diag_->status[0].level)
<< "message: " << diag_->status[0].message;
}
ASSERT_TRUE(received);
sub_cmd_vel.shutdown();
}
Expand Down Expand Up @@ -354,6 +393,10 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngular)
wait.sleep();
ros::spinOnce();
}
ASSERT_TRUE(hasDiag());
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
<< "message: " << diag_->status[0].message;

ASSERT_TRUE(received);
sub_cmd_vel.shutdown();
}
Expand Down Expand Up @@ -388,11 +431,15 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngularEscape)
received = true;
failed = true;
if (vel < 0)
{
// escaping from collision must be allowed
ASSERT_NEAR(msg->angular.z, vel, 1e-1);
}
else
{
// colliding motion must be limited
ASSERT_NEAR(msg->angular.z, 0.0, 1e-1);
}
failed = false;
};
ros::Subscriber sub_cmd_vel = nh_.subscribe("cmd_vel", 1, cb_cmd_vel);
Expand All @@ -408,6 +455,18 @@ TEST_F(SafetyLimiterTest, SafetyLimitAngularEscape)
wait.sleep();
ros::spinOnce();
}
if (vel < 0)
{
ASSERT_TRUE(hasDiag());
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
<< "message: " << diag_->status[0].message;
}
else
{
ASSERT_TRUE(hasDiag());
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::WARN, diag_->status[0].level)
<< "message: " << diag_->status[0].message;
}
ASSERT_TRUE(received);
sub_cmd_vel.shutdown();
}
Expand Down Expand Up @@ -447,6 +506,10 @@ TEST_F(SafetyLimiterTest, NoCollision)
ros::spinOnce();
}
ASSERT_TRUE(received);

ASSERT_TRUE(hasDiag());
EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, diag_->status[0].level)
<< "message: " << diag_->status[0].message;
sub_cmd_vel.shutdown();
}
}
Expand Down