Skip to content

Commit

Permalink
Fix velocities comparison for rotation at place case
Browse files Browse the repository at this point in the history
  • Loading branch information
@AlexeyMerzlyakov
AlexeyMerzlyakov committed Sep 7, 2022
1 parent 42d0bbd commit 6e54086
Show file tree
Hide file tree
Showing 2 changed files with 130 additions and 10 deletions.
12 changes: 11 additions & 1 deletion nav2_collision_monitor/include/nav2_collision_monitor/types.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef NAV2_COLLISION_MONITOR__TYPES_HPP_
#define NAV2_COLLISION_MONITOR__TYPES_HPP_

#include <cmath>

namespace nav2_collision_monitor
{

Expand All @@ -29,7 +31,15 @@ struct Velocity
{
const double first_vel = x * x + y * y;
const double second_vel = second.x * second.x + second.y * second.y;
return first_vel < second_vel;
if (second_vel > 0.0) {
// Robot moves: comparing velocities. In Collision Monitor
// twists will change proportionally to linear velocities.
// We do not need to compare them in this case.
return first_vel < second_vel;
} else {
// Robot is already stays in place. We need to compare twists.
return std::fabs(tw) < std::fabs(second.tw);
}
}

inline Velocity operator*(const double & mul) const
Expand Down
128 changes: 119 additions & 9 deletions nav2_collision_monitor/test/collision_monitor_node_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include "sensor_msgs/msg/range.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/polygon_stamped.hpp"

#include "tf2_ros/transform_broadcaster.h"

Expand All @@ -38,13 +39,14 @@

using namespace std::chrono_literals;

static constexpr double EPSILON = std::numeric_limits<float>::epsilon();
static constexpr double EPSILON = 1e-5;

static const char BASE_FRAME_ID[]{"base_link"};
static const char SOURCE_FRAME_ID[]{"base_source"};
static const char ODOM_FRAME_ID[]{"odom"};
static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"};
static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"};
static const char FOOTPRINT_TOPIC[]{"footprint"};
static const char SCAN_NAME[]{"Scan"};
static const char POINTCLOUD_NAME[]{"PointCloud"};
static const char RANGE_NAME[]{"Range"};
Expand Down Expand Up @@ -132,6 +134,9 @@ class Tester : public ::testing::Test
// Setting TF chains
void sendTransforms(const rclcpp::Time & stamp);

// Publish robot footprint
void publishFootprint(const double radius, const rclcpp::Time & stamp);

// Main topic/data working routines
void publishScan(const double dist, const rclcpp::Time & stamp);
void publishPointCloud(const double dist, const rclcpp::Time & stamp);
Expand All @@ -149,6 +154,9 @@ class Tester : public ::testing::Test
// CollisionMonitor node
std::shared_ptr<CollisionMonitorWrapper> cm_;

// Footprint publisher
rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;

// Data source publishers
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
Expand All @@ -165,6 +173,9 @@ Tester::Tester()
{
cm_ = std::make_shared<CollisionMonitorWrapper>();

footprint_pub_ = cm_->create_publisher<geometry_msgs::msg::PolygonStamped>(
FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());

scan_pub_ = cm_->create_publisher<sensor_msgs::msg::LaserScan>(
SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
pointcloud_pub_ = cm_->create_publisher<sensor_msgs::msg::PointCloud2>(
Expand All @@ -181,6 +192,8 @@ Tester::Tester()

Tester::~Tester()
{
footprint_pub_.reset();

scan_pub_.reset();
pointcloud_pub_.reset();
range_pub_.reset();
Expand Down Expand Up @@ -236,12 +249,19 @@ void Tester::addPolygon(
cm_->set_parameter(
rclcpp::Parameter(polygon_name + ".type", "polygon"));

const std::vector<double> points {
size, size, size, -size, -size, -size, -size, size};
cm_->declare_parameter(
polygon_name + ".points", rclcpp::ParameterValue(points));
cm_->set_parameter(
rclcpp::Parameter(polygon_name + ".points", points));
if (at != "approach") {
const std::vector<double> points {
size, size, size, -size, -size, -size, -size, size};
cm_->declare_parameter(
polygon_name + ".points", rclcpp::ParameterValue(points));
cm_->set_parameter(
rclcpp::Parameter(polygon_name + ".points", points));
} else { // at == "approach"
cm_->declare_parameter(
polygon_name + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC));
cm_->set_parameter(
rclcpp::Parameter(polygon_name + ".footprint_topic", FOOTPRINT_TOPIC));
}
} else if (type == CIRCLE) {
cm_->declare_parameter(
polygon_name + ".type", rclcpp::ParameterValue("circle"));
Expand Down Expand Up @@ -379,6 +399,31 @@ void Tester::sendTransforms(const rclcpp::Time & stamp)
}
}

void Tester::publishFootprint(const double radius, const rclcpp::Time & stamp)
{
std::unique_ptr<geometry_msgs::msg::PolygonStamped> msg =
std::make_unique<geometry_msgs::msg::PolygonStamped>();

msg->header.frame_id = BASE_FRAME_ID;
msg->header.stamp = stamp;

geometry_msgs::msg::Point32 p;
p.x = radius;
p.y = radius;
msg->polygon.points.push_back(p);
p.x = radius;
p.y = -radius;
msg->polygon.points.push_back(p);
p.x = -radius;
p.y = -radius;
msg->polygon.points.push_back(p);
p.x = -radius;
p.y = radius;
msg->polygon.points.push_back(p);

footprint_pub_->publish(std::move(msg));
}

void Tester::publishScan(const double dist, const rclcpp::Time & stamp)
{
std::unique_ptr<sensor_msgs::msg::LaserScan> msg =
Expand Down Expand Up @@ -544,7 +589,7 @@ TEST_F(Tester, testProcessStopSlowdown)
ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON);

// 4. Restorint back normal operation
// 4. Restoring back normal operation
publishScan(3.0, curr_time);
ASSERT_TRUE(waitData(3.0, 500ms, curr_time));
publishCmdVel(0.5, 0.2, 0.1);
Expand Down Expand Up @@ -606,7 +651,7 @@ TEST_F(Tester, testProcessApproach)
ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON);

// 4. Restorint back normal operation
// 4. Restoring back normal operation
publishScan(3.0, curr_time);
ASSERT_TRUE(waitData(3.0, 500ms, curr_time));
publishCmdVel(0.5, 0.2, 0.0);
Expand All @@ -619,6 +664,71 @@ TEST_F(Tester, testProcessApproach)
cm_->stop();
}

TEST_F(Tester, testProcessApproachRotation)
{
rclcpp::Time curr_time = cm_->now();

// Set Collision Monitor parameters.
// Making one circle footprint for approach.
setCommonParameters();
addPolygon("Approach", POLYGON, 1.0, "approach");
addSource(RANGE_NAME, RANGE);
setVectors({"Approach"}, {RANGE_NAME});

// Start Collision Monitor node
cm_->start();

// Publish robot footprint
publishFootprint(1.0, curr_time);

// Share TF
sendTransforms(curr_time);

// 1. Obstacle is far away from robot
publishRange(2.0, curr_time);
ASSERT_TRUE(waitData(2.0, 500ms, curr_time));
publishCmdVel(0.0, 0.0, M_PI / 4);
ASSERT_TRUE(waitCmdVel(500ms));
ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON);
ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
ASSERT_NEAR(cmd_vel_out_->angular.z, M_PI / 4, EPSILON);

// 2. Approaching rotation to obstacle ( M_PI / 4 - M_PI / 20 ahead from robot)
publishRange(1.4, curr_time);
ASSERT_TRUE(waitData(1.4, 500ms, curr_time));
publishCmdVel(0.0, 0.0, M_PI / 4);
ASSERT_TRUE(waitCmdVel(500ms));
ASSERT_NEAR(
cmd_vel_out_->linear.x, 0.0, EPSILON);
ASSERT_NEAR(
cmd_vel_out_->linear.y, 0.0, EPSILON);
ASSERT_NEAR(
cmd_vel_out_->angular.z,
M_PI / 5,
(M_PI / 4) * (SIMULATION_TIME_STEP / TIME_BEFORE_COLLISION));

// 3. Obstacle is inside robot footprint
publishRange(0.5, curr_time);
ASSERT_TRUE(waitData(0.5, 500ms, curr_time));
publishCmdVel(0.0, 0.0, M_PI / 4);
ASSERT_TRUE(waitCmdVel(500ms));
ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON);
ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON);

// 4. Restoring back normal operation
publishRange(2.5, curr_time);
ASSERT_TRUE(waitData(2.5, 500ms, curr_time));
publishCmdVel(0.0, 0.0, M_PI / 4);
ASSERT_TRUE(waitCmdVel(500ms));
ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON);
ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
ASSERT_NEAR(cmd_vel_out_->angular.z, M_PI / 4, EPSILON);

// Stop Collision Monitor node
cm_->stop();
}

TEST_F(Tester, testCrossOver)
{
rclcpp::Time curr_time = cm_->now();
Expand Down

0 comments on commit 6e54086

Please sign in to comment.