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

safety_limiter: Add a max_linear_vel and max_angular_vel #581

Merged
merged 6 commits into from
Feb 3, 2021
Merged

safety_limiter: Add a max_linear_vel and max_angular_vel #581

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
ce39449
Add linear and angular max values.
Teo-Cardoso Feb 2, 2021
f8de923
Add negative min("max") values too.
Teo-Cardoso Feb 2, 2021
3056e64
Create MaxValues test.
Teo-Cardoso Feb 2, 2021
4f74037
Add max values to dynamic reconfigure.
Teo-Cardoso Feb 2, 2021
1b9ee36
Fix roslint erros.
Teo-Cardoso Feb 2, 2021
9bf011a
Fix gtest args position.
Teo-Cardoso Feb 3, 2021
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
2 changes: 2 additions & 0 deletions safety_limiter/cfg/SafetyLimiter.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,10 @@ gen.add("cloud_timeout", double_t, 0, "", 0.8, 0.0, 10.0)
gen.add("disable_timeout", double_t, 0, "", 0.1, 0.0, 10.0)
gen.add("lin_vel", double_t, 0, "", 0.5, 0.0, 10.0)
gen.add("lin_acc", double_t, 0, "", 1.0, 0.0, 10.0)
gen.add("max_linear_vel", double_t, 0, "", 10.0, 0.0, 10.0)
gen.add("ang_vel", double_t, 0, "", 0.8, 0.0, 10.0)
gen.add("ang_acc", double_t, 0, "", 1.6, 0.0, 10.0)
gen.add("max_angular_vel", double_t, 0, "", 10.0, 0.0, 10.0)
gen.add("z_range_min", double_t, 0, "", 0.0, -3.0, 3.0)
gen.add("z_range_max", double_t, 0, "", 0.5, -3.0, 3.0)
gen.add("dt", double_t, 0, "", 0.1, 0.0, 1.0)
Expand Down
25 changes: 23 additions & 2 deletions safety_limiter/src/safety_limiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,7 @@ class SafetyLimiterNode
double yaw_margin_;
double yaw_escape_;
double r_lim_;
double max_values_[2];
double z_range_[2];
float footprint_radius_;
double downsample_grid_;
Expand Down Expand Up @@ -208,6 +209,8 @@ class SafetyLimiterNode
double watchdog_interval_d;
pnh_.param("watchdog_interval", watchdog_interval_d, 0.0);
watchdog_interval_ = ros::Duration(watchdog_interval_d);
pnh_.param("max_linear_vel", max_values_[0], std::numeric_limits<double>::infinity());
pnh_.param("max_angular_vel", max_values_[1], std::numeric_limits<double>::infinity());

parameter_server_.reset(
new dynamic_reconfigure::Server<SafetyLimiterConfig>(parameter_server_mutex_, pnh_));
Expand Down Expand Up @@ -324,6 +327,8 @@ class SafetyLimiterNode
acc_[0] = config.lin_acc;
vel_[1] = config.ang_vel;
acc_[1] = config.ang_acc;
max_values_[0] = config.max_linear_vel;
max_values_[1] = config.max_angular_vel;
z_range_[0] = config.z_range_min;
z_range_[1] = config.z_range_max;
dt_ = config.dt;
Expand Down Expand Up @@ -567,6 +572,21 @@ class SafetyLimiterNode
return out;
}

geometry_msgs::Twist
limitMaxVelocities(const geometry_msgs::Twist& in)
{
auto out = in;
out.linear.x = (out.linear.x > 0) ?
std::min(out.linear.x, max_values_[0]) :
std::max(out.linear.x, -max_values_[0]);

out.angular.z = (out.angular.z > 0) ?
std::min(out.angular.z, max_values_[1]) :
std::max(out.angular.z, -max_values_[1]);

return out;
}

class vec
{
public:
Expand Down Expand Up @@ -681,7 +701,7 @@ class SafetyLimiterNode

if (now - last_disable_cmd_ < ros::Duration(disable_timeout_))
{
pub_twist_.publish(twist_);
pub_twist_.publish(limitMaxVelocities(twist_));
}
else if (!has_cloud_ || watchdog_stop_)
{
Expand All @@ -690,13 +710,14 @@ class SafetyLimiterNode
}
else
{
geometry_msgs::Twist cmd_vel = limit(twist_);
geometry_msgs::Twist cmd_vel = limitMaxVelocities(limit(twist_));
pub_twist_.publish(cmd_vel);

if (now > hold_off_)
r_lim_ = 1.0;
}
}

void cbCloud(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
const bool can_transform = tfbuf_.canTransform(
Expand Down
53 changes: 53 additions & 0 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 @@ -559,6 +559,59 @@ TEST_F(SafetyLimiterTest, SafetyLimitLinearSimpleSimulation)
}
}

TEST_F(SafetyLimiterTest, SafetyLimitMaxVelocitiesValues)
{
ros::Rate wait(20);

const float linear_velocities[] =
{-1.7, -1.5, 0.0, 1.5, 1.7 };
const float angular_velocities[] =
{-2.7, -2.5, 0.0, 2.5, 2.7 };
const float expected_linear_velocities[] =
{-1.5, -1.5, 0.0, 1.5, 1.5 };
const float expected_angular_velocities[] =
{-2.5, -2.5, 0.0, 2.5, 2.5 };

for (int linear_index = 0; linear_index < 5; linear_index++)
{
for (int angular_index = 0; angular_index < 5; angular_index++)
{
bool received = false;
bool en = false;

for (int i = 0; i < 10 && ros::ok(); ++i)
{
if (i > 5) en = true;
publishSinglePointPointcloud2(1000, 1000, 0, "base_link", ros::Time::now());
publishWatchdogReset();
publishTwist(linear_velocities[linear_index], angular_velocities[angular_index]);
broadcastTF("odom", "base_link", 0.0, 0.0);

wait.sleep();
ros::spinOnce();
if (en && cmd_vel_)
{
received = true;
ASSERT_NEAR(expected_linear_velocities[linear_index], cmd_vel_->linear.x, 1e-3);
ASSERT_NEAR(expected_angular_velocities[angular_index], cmd_vel_->angular.z, 1e-3);
}
}
ASSERT_TRUE(received);

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

ASSERT_TRUE(hasStatus());
EXPECT_EQ(1.0, status_->limit_ratio);
EXPECT_TRUE(status_->is_cloud_available);
EXPECT_FALSE(status_->has_watchdog_timed_out);
EXPECT_EQ(status_->stuck_started_since, ros::Time(0));
}
}
}


int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
Expand Down
4 changes: 3 additions & 1 deletion safety_limiter/test/test/safety_limiter_compat_rostest.test
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
<env name="GCOV_PREFIX" value="/tmp/gcov/safety_limiter_safert_limiter_compat" />
<param name="neonavigation_compatible" value="0" />

<test test-name="test_safety_limiter" pkg="safety_limiter" type="test_safety_limiter" time-limit="90.0">
<test test-name="test_safety_limiter" pkg="safety_limiter" type="test_safety_limiter" time-limit="100.0">
<remap from="cmd_vel_in" to="/safety_limiter/cmd_vel_in" />
<remap from="cloud" to="/safety_limiter/cloud" />
<remap from="watchdog_reset" to="/safety_limiter/watchdog_reset" />
Expand All @@ -14,6 +14,8 @@
<rosparam param="footprint">[[0.0, -0.1], [0.0, 0.1], [-2.0, 0.1], [-2.0, -0.1]]</rosparam>
<param name="allow_empty_cloud" value="false" />
<param name="watchdog_interval" value="0.2" />
<param name="max_linear_vel" value="1.5"/>
<param name="max_angular_vel" value="2.5"/>
<param name="cloud_timeout" value="0.2" />
<param name="freq" value="15.0" />
<param name="dt" value="0.01" />
Expand Down
4 changes: 3 additions & 1 deletion safety_limiter/test/test/safety_limiter_rostest.test
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,12 +3,14 @@
<env name="GCOV_PREFIX" value="/tmp/gcov/safety_limiter_safert_limiter" />
<param name="neonavigation_compatible" value="1" />

<test test-name="test_safety_limiter" pkg="safety_limiter" type="test_safety_limiter" time-limit="90.0" />
<test test-name="test_safety_limiter" pkg="safety_limiter" type="test_safety_limiter" time-limit="100.0" />

<node pkg="safety_limiter" type="safety_limiter" name="safety_limiter" output="screen">
<rosparam param="footprint">[[0.0, -0.1], [0.0, 0.1], [-2.0, 0.1], [-2.0, -0.1]]</rosparam>
<param name="allow_empty_cloud" value="false" />
<param name="watchdog_interval" value="0.2" />
<param name="max_linear_vel" value="1.5"/>
<param name="max_angular_vel" value="2.5"/>
<param name="cloud_timeout" value="0.2" />
<param name="freq" value="15.0" />
<param name="dt" value="0.01" />
Expand Down