add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,112 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_collision_monitor)
|
||||
|
||||
### Dependencies ###
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_components REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
find_package(nav2_costmap_2d REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
|
||||
### Header ###
|
||||
|
||||
nav2_package()
|
||||
|
||||
### Libraries and executables ###
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
set(dependencies
|
||||
rclcpp
|
||||
rclcpp_components
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
tf2
|
||||
tf2_ros
|
||||
tf2_geometry_msgs
|
||||
nav2_util
|
||||
nav2_costmap_2d
|
||||
nav2_msgs
|
||||
)
|
||||
|
||||
set(executable_name collision_monitor)
|
||||
set(library_name ${executable_name}_core)
|
||||
|
||||
add_library(${library_name} SHARED
|
||||
src/collision_monitor_node.cpp
|
||||
src/polygon.cpp
|
||||
src/circle.cpp
|
||||
src/source.cpp
|
||||
src/scan.cpp
|
||||
src/pointcloud.cpp
|
||||
src/range.cpp
|
||||
src/kinematics.cpp
|
||||
)
|
||||
|
||||
add_executable(${executable_name}
|
||||
src/main.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${library_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
target_link_libraries(${executable_name}
|
||||
${library_name}
|
||||
)
|
||||
|
||||
ament_target_dependencies(${executable_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
rclcpp_components_register_nodes(${library_name} "nav2_collision_monitor::CollisionMonitor")
|
||||
|
||||
### Install ###
|
||||
|
||||
install(TARGETS ${library_name}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(TARGETS ${executable_name}
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
|
||||
install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
### Testing ###
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
add_subdirectory(test)
|
||||
endif()
|
||||
|
||||
### Ament stuff ###
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(${library_name})
|
||||
ament_export_dependencies(${dependencies})
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,67 @@
|
||||
# Nav2 Collision Monitor
|
||||
|
||||
The Collision Monitor is a node providing an additional level of robot safety.
|
||||
It performs several collision avoidance related tasks using incoming data from the sensors, bypassing the costmap and trajectory planners, to monitor for and prevent potential collisions at the emergency-stop level.
|
||||
|
||||
This is analogous to safety sensor and hardware features; take in laser scans from a real-time certified safety scanner, detect if there is to be an imminent collision in a configurable bounding box, and either emergency-stop the certified robot controller or slow the robot to avoid such collision.
|
||||
However, this node is done at the CPU level with any form of sensor.
|
||||
As such, this does not provide hard real-time safety certifications, but uses the same types of techniques with the same types of data for users that do not have safety-rated laser sensors, safety-rated controllers, or wish to use any type of data input (e.g. pointclouds from depth or stereo or range sensors).
|
||||
|
||||
This is a useful and integral part of large heavy industrial robots, or robots moving with high velocities, around people or other dynamic agents (e.g. other robots) as a safety mechanism for high-response emergency stopping.
|
||||
The costmaps / trajectory planners will handle most situations, but this is to handle obstacles that virtually appear out of no where (from the robot's perspective) or approach the robot at such high speed it needs to immediately stop to prevent collision.
|
||||
|
||||
|
||||
|
||||
## Features
|
||||
|
||||
The Collision Monitor uses polygons relative the robot's base frame origin to define "zones".
|
||||
Data that fall into these zones trigger an operation depending on the model being used.
|
||||
A given instance of the Collision Monitor can have many zones with different models at the same time.
|
||||
When multiple zones trigger at once, the most aggressive one is used (e.g. stop > slow 50% > slow 10%).
|
||||
|
||||
The following models of safety behaviors are employed by Collision Monitor:
|
||||
|
||||
* **Stop model**: Define a zone and a point threshold. If more that `N` obstacle points appear inside this area, stop the robot until the obstacles will disappear.
|
||||
* **Slowdown model**: Define a zone around the robot and slow the maximum speed for a `%S` percent, if more than `N` points will appear inside the area.
|
||||
* **Approach model**: Using the current robot speed, estimate the time to collision to sensor data. If the time is less than `M` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least `M` seconds to collision. The effect here would be to keep the robot always `M` seconds from any collision.
|
||||
|
||||
The zones around the robot can take the following shapes:
|
||||
|
||||
* Arbitrary user-defined polygon relative to the robot base frame.
|
||||
* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape.
|
||||
* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time.
|
||||
|
||||
The data may be obtained from different data sources:
|
||||
|
||||
* Laser scanners (`sensor_msgs::msg::LaserScan` messages)
|
||||
* PointClouds (`sensor_msgs::msg::PointCloud2` messages)
|
||||
* IR/Sonars (`sensor_msgs::msg::Range` messages)
|
||||
|
||||
## Design
|
||||
|
||||
The Collision Monitor is designed to operate below Nav2 as an independent safety node.
|
||||
This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate.
|
||||
|
||||
The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model.
|
||||
|
||||
|
||||
## Configuration
|
||||
|
||||
Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages.
|
||||
|
||||
|
||||
## Metrics
|
||||
|
||||
Designed to be used in wide variety of robots (incl. moving fast) and have a high level of reliability, Collision Monitor node should operate at fast rates.
|
||||
Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points).
|
||||
The table below represents the details of operating times for different behavior models and shapes:
|
||||
|
||||
| | Stop/Slowdown model, Polygon area | Stop/Slowdown model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint |
|
||||
|-|-----------------------------------|----------------------------------|-----------------------------------|----------------------------------|
|
||||
| LaserScan (360 points) processing time, ms | 4.45 | 4.45 | 4.93 | 4.86 |
|
||||
| PointCloud (24K points) processing time, ms | 4.94 | 4.06 | 20.67 | 10.87 |
|
||||
|
||||
The following notes could be made:
|
||||
|
||||
* Due to sheer speed, circle shapes are preferred for the approach behavior models if you can approximately model your robot as circular.
|
||||
* More points mean lower performance. Pointclouds could be culled or filtered before the Collision Monitor to improve performance.
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 64 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 540 KiB |
@@ -0,0 +1,89 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_COLLISION_MONITOR__CIRCLE_HPP_
|
||||
#define NAV2_COLLISION_MONITOR__CIRCLE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_collision_monitor/polygon.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Circle shape implementaiton.
|
||||
* For STOP/SLOWDOWN model it represents zone around the robot
|
||||
* while for APPROACH model it represents robot footprint.
|
||||
*/
|
||||
class Circle : public Polygon
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Circle class constructor
|
||||
* @param node Collision Monitor node pointer
|
||||
* @param polygon_name Name of circle
|
||||
* @param tf_buffer Shared pointer to a TF buffer
|
||||
* @param base_frame_id Robot base frame ID
|
||||
* @param transform_tolerance Transform tolerance
|
||||
*/
|
||||
Circle(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & polygon_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const tf2::Duration & transform_tolerance);
|
||||
/**
|
||||
* @brief Circle class destructor
|
||||
*/
|
||||
~Circle();
|
||||
|
||||
/**
|
||||
* @brief Gets polygon points, approximated to the circle.
|
||||
* To be used in visualization purposes.
|
||||
* @param poly Output polygon points (vertices)
|
||||
*/
|
||||
void getPolygon(std::vector<Point> & poly) const override;
|
||||
|
||||
/**
|
||||
* @brief Gets number of points inside circle
|
||||
* @param points Input array of points to be checked
|
||||
* @return Number of points inside circle. If there are no points,
|
||||
* returns zero value.
|
||||
*/
|
||||
int getPointsInside(const std::vector<Point> & points) const override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Supporting routine obtaining polygon-specific ROS-parameters
|
||||
* @param polygon_pub_topic Output name of polygon publishing topic
|
||||
* @param footprint_topic Output name of footprint topic. For Circle returns empty string,
|
||||
* there is no footprint subscription in this class.
|
||||
* @return True if all parameters were obtained or false in failure case
|
||||
*/
|
||||
bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) override;
|
||||
|
||||
// ----- Variables -----
|
||||
|
||||
/// @brief Radius of the circle
|
||||
double radius_;
|
||||
/// @brief (radius * radius) value. Stored for optimization.
|
||||
double radius_squared_;
|
||||
}; // class Circle
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#endif // NAV2_COLLISION_MONITOR__CIRCLE_HPP_
|
||||
+221
@@ -0,0 +1,221 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_COLLISION_MONITOR__COLLISION_MONITOR_NODE_HPP_
|
||||
#define NAV2_COLLISION_MONITOR__COLLISION_MONITOR_NODE_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
|
||||
#include "tf2/time.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/types.hpp"
|
||||
#include "nav2_collision_monitor/polygon.hpp"
|
||||
#include "nav2_collision_monitor/circle.hpp"
|
||||
#include "nav2_collision_monitor/source.hpp"
|
||||
#include "nav2_collision_monitor/scan.hpp"
|
||||
#include "nav2_collision_monitor/pointcloud.hpp"
|
||||
#include "nav2_collision_monitor/range.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Collision Monitor ROS2 node
|
||||
*/
|
||||
class CollisionMonitor : public nav2_util::LifecycleNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for the nav2_collision_safery::CollisionMonitor
|
||||
* @param options Additional options to control creation of the node.
|
||||
*/
|
||||
explicit CollisionMonitor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
||||
/**
|
||||
* @brief Destructor for the nav2_collision_safery::CollisionMonitor
|
||||
*/
|
||||
~CollisionMonitor();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief: Initializes and obtains ROS-parameters, creates main subscribers and publishers,
|
||||
* creates polygons and data sources objects
|
||||
* @param state Lifecycle Node's state
|
||||
* @return Success or Failure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief: Activates LifecyclePublishers, polygons and main processor, creates bond connection
|
||||
* @param state Lifecycle Node's state
|
||||
* @return Success or Failure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection
|
||||
* @param state Lifecycle Node's state
|
||||
* @return Success or Failure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief: Resets all subscribers/publishers, polygons/data sources arrays
|
||||
* @param state Lifecycle Node's state
|
||||
* @return Success or Failure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
|
||||
/**
|
||||
* @brief Called in shutdown state
|
||||
* @param state Lifecycle Node's state
|
||||
* @return Success or Failure
|
||||
*/
|
||||
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Callback for input cmd_vel
|
||||
* @param msg Input cmd_vel message
|
||||
*/
|
||||
void cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg);
|
||||
/**
|
||||
* @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds,
|
||||
* quit to publish 0-velocity.
|
||||
* @param robot_action Robot action to publish
|
||||
*/
|
||||
void publishVelocity(const Action & robot_action);
|
||||
|
||||
/**
|
||||
* @brief Supporting routine obtaining all ROS-parameters
|
||||
* @param cmd_vel_in_topic Output name of cmd_vel_in topic
|
||||
* @param cmd_vel_out_topic Output name of cmd_vel_out topic
|
||||
* is required.
|
||||
* @return True if all parameters were obtained or false in failure case
|
||||
*/
|
||||
bool getParameters(
|
||||
std::string & cmd_vel_in_topic,
|
||||
std::string & cmd_vel_out_topic);
|
||||
/**
|
||||
* @brief Supporting routine creating and configuring all polygons
|
||||
* @param base_frame_id Robot base frame ID
|
||||
* @param transform_tolerance Transform tolerance
|
||||
* @return True if all polygons were configured successfully or false in failure case
|
||||
*/
|
||||
bool configurePolygons(
|
||||
const std::string & base_frame_id,
|
||||
const tf2::Duration & transform_tolerance);
|
||||
/**
|
||||
* @brief Supporting routine creating and configuring all data sources
|
||||
* @param base_frame_id Robot base frame ID
|
||||
* @param odom_frame_id Odometry frame ID. Used as global frame to get
|
||||
* source->base time inerpolated transform.
|
||||
* @param transform_tolerance Transform tolerance
|
||||
* @param source_timeout Maximum time interval in which data is considered valid
|
||||
* @param base_shift_correction Whether to correct source data towards to base frame movement,
|
||||
* considering the difference between current time and latest source time
|
||||
* @return True if all sources were configured successfully or false in failure case
|
||||
*/
|
||||
bool configureSources(
|
||||
const std::string & base_frame_id,
|
||||
const std::string & odom_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction);
|
||||
|
||||
/**
|
||||
* @brief Main processing routine
|
||||
* @param cmd_vel_in Input desired robot velocity
|
||||
*/
|
||||
void process(const Velocity & cmd_vel_in);
|
||||
|
||||
/**
|
||||
* @brief Processes the polygon of STOP and SLOWDOWN action type
|
||||
* @param polygon Polygon to process
|
||||
* @param collision_points Array of 2D obstacle points
|
||||
* @param velocity Desired robot velocity
|
||||
* @param robot_action Output processed robot action
|
||||
* @return True if returned action is caused by current polygon, otherwise false
|
||||
*/
|
||||
bool processStopSlowdown(
|
||||
const std::shared_ptr<Polygon> polygon,
|
||||
const std::vector<Point> & collision_points,
|
||||
const Velocity & velocity,
|
||||
Action & robot_action) const;
|
||||
|
||||
/**
|
||||
* @brief Processes APPROACH action type
|
||||
* @param polygon Polygon to process
|
||||
* @param collision_points Array of 2D obstacle points
|
||||
* @param velocity Desired robot velocity
|
||||
* @param robot_action Output processed robot action
|
||||
* @return True if returned action is caused by current polygon, otherwise false
|
||||
*/
|
||||
bool processApproach(
|
||||
const std::shared_ptr<Polygon> polygon,
|
||||
const std::vector<Point> & collision_points,
|
||||
const Velocity & velocity,
|
||||
Action & robot_action) const;
|
||||
|
||||
/**
|
||||
* @brief Prints robot action and polygon caused it (if it was)
|
||||
* @param robot_action Robot action to print
|
||||
* @param action_polygon Pointer to a polygon causing a selected action
|
||||
*/
|
||||
void printAction(
|
||||
const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const;
|
||||
|
||||
/**
|
||||
* @brief Polygons publishing routine. Made for visualization.
|
||||
*/
|
||||
void publishPolygons() const;
|
||||
|
||||
// ----- Variables -----
|
||||
|
||||
/// @brief TF buffer
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
/// @brief TF listener
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
|
||||
/// @brief Polygons array
|
||||
std::vector<std::shared_ptr<Polygon>> polygons_;
|
||||
|
||||
/// @brief Data sources array
|
||||
std::vector<std::shared_ptr<Source>> sources_;
|
||||
|
||||
// Input/output speed controls
|
||||
/// @beirf Input cmd_vel subscriber
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_in_sub_;
|
||||
/// @brief Output cmd_vel publisher
|
||||
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_;
|
||||
|
||||
/// @brief Whether main routine is active
|
||||
bool process_active_;
|
||||
|
||||
/// @brief Previous robot action
|
||||
Action robot_action_prev_;
|
||||
/// @brief Latest timestamp when robot has 0-velocity
|
||||
rclcpp::Time stop_stamp_;
|
||||
/// @brief Timeout after which 0-velocity ceases to be published
|
||||
rclcpp::Duration stop_pub_timeout_;
|
||||
}; // class CollisionMonitor
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#endif // NAV2_COLLISION_MONITOR__COLLISION_MONITOR_NODE_HPP_
|
||||
@@ -0,0 +1,46 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_COLLISION_MONITOR__KINEMATICS_HPP_
|
||||
#define NAV2_COLLISION_MONITOR__KINEMATICS_HPP_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_collision_monitor/types.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Do a transformation of points' coordinates from the frame coinciding with the (0,0)
|
||||
* origin to the non-existing in ROS frame, which origin is equal to pose
|
||||
* @param pose Origin of the new frame
|
||||
* @param points Array of points whose coordinates will be transformed
|
||||
*/
|
||||
void transformPoints(const Pose & pose, std::vector<Point> & points);
|
||||
|
||||
/**
|
||||
* @brief Linearly projects pose towards to velocity direction on dt time interval.
|
||||
* Turns the velocity on twist angle for dt time interval.
|
||||
* @param dt Time step (in seconds). Should be relatively small
|
||||
* to consider all movements to be linear.
|
||||
* @param pose Pose to be projected
|
||||
* @param velocity Velocity at which the pose to be moved. It is also being rotated
|
||||
* on according twist angle.
|
||||
*/
|
||||
void projectState(const double & dt, Pose & pose, Velocity & velocity);
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#endif // NAV2_COLLISION_MONITOR__KINEMATICS_HPP_
|
||||
@@ -0,0 +1,105 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_COLLISION_MONITOR__POINTCLOUD_HPP_
|
||||
#define NAV2_COLLISION_MONITOR__POINTCLOUD_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/source.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Implementation for pointcloud source
|
||||
*/
|
||||
class PointCloud : public Source
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief PointCloud constructor
|
||||
* @param node Collision Monitor node pointer
|
||||
* @param source_name Name of data source
|
||||
* @param tf_buffer Shared pointer to a TF buffer
|
||||
* @param base_frame_id Robot base frame ID. The output data will be transformed into this frame.
|
||||
* @param global_frame_id Global frame ID for correct transform calculation
|
||||
* @param transform_tolerance Transform tolerance
|
||||
* @param source_timeout Maximum time interval in which data is considered valid
|
||||
* @param base_shift_correction Whether to correct source data towards to base frame movement,
|
||||
* considering the difference between current time and latest source time
|
||||
*/
|
||||
PointCloud(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction);
|
||||
/**
|
||||
* @brief PointCloud destructor
|
||||
*/
|
||||
~PointCloud();
|
||||
|
||||
/**
|
||||
* @brief Data source configuration routine. Obtains pointcloud related ROS-parameters
|
||||
* and creates pointcloud subscriber.
|
||||
*/
|
||||
void configure();
|
||||
|
||||
/**
|
||||
* @brief Adds latest data from pointcloud source to the data array.
|
||||
* @param curr_time Current node time for data interpolation
|
||||
* @param data Array where the data from source to be added.
|
||||
* Added data is transformed to base_frame_id_ coordinate system at curr_time.
|
||||
*/
|
||||
void getData(
|
||||
const rclcpp::Time & curr_time,
|
||||
std::vector<Point> & data) const;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Getting sensor-specific ROS-parameters
|
||||
* @param source_topic Output name of source subscription topic
|
||||
*/
|
||||
void getParameters(std::string & source_topic);
|
||||
|
||||
/**
|
||||
* @brief PointCloud data callback
|
||||
* @param msg Shared pointer to PointCloud message
|
||||
*/
|
||||
void dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
|
||||
|
||||
// ----- Variables -----
|
||||
|
||||
/// @brief PointCloud data subscriber
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr data_sub_;
|
||||
|
||||
// Minimum and maximum height of PointCloud projected to 2D space
|
||||
double min_height_, max_height_;
|
||||
|
||||
/// @brief Latest data obtained from pointcloud
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr data_;
|
||||
}; // class PointCloud
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#endif // NAV2_COLLISION_MONITOR__POINTCLOUD_HPP_
|
||||
@@ -0,0 +1,228 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_COLLISION_MONITOR__POLYGON_HPP_
|
||||
#define NAV2_COLLISION_MONITOR__POLYGON_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "geometry_msgs/msg/polygon_stamped.hpp"
|
||||
#include "geometry_msgs/msg/polygon.hpp"
|
||||
|
||||
#include "tf2/time.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_costmap_2d/footprint_subscriber.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/types.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Basic polygon shape class.
|
||||
* For STOP/SLOWDOWN model it represents zone around the robot
|
||||
* while for APPROACH model it represents robot footprint.
|
||||
*/
|
||||
class Polygon
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Polygon constructor
|
||||
* @param node Collision Monitor node pointer
|
||||
* @param polygon_name Name of polygon
|
||||
* @param tf_buffer Shared pointer to a TF buffer
|
||||
* @param base_frame_id Robot base frame ID
|
||||
* @param transform_tolerance Transform tolerance
|
||||
*/
|
||||
Polygon(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & polygon_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const tf2::Duration & transform_tolerance);
|
||||
/**
|
||||
* @brief Polygon destructor
|
||||
*/
|
||||
virtual ~Polygon();
|
||||
|
||||
/**
|
||||
* @brief Shape configuration routine. Obtains ROS-parameters related to shape object
|
||||
* and creates polygon lifecycle publisher.
|
||||
* @return True in case of everything is configured correctly, or false otherwise
|
||||
*/
|
||||
bool configure();
|
||||
/**
|
||||
* @brief Activates polygon lifecycle publisher
|
||||
*/
|
||||
void activate();
|
||||
/**
|
||||
* @brief Deactivates polygon lifecycle publisher
|
||||
*/
|
||||
void deactivate();
|
||||
|
||||
/**
|
||||
* @brief Returns the name of polygon
|
||||
* @return Polygon name
|
||||
*/
|
||||
std::string getName() const;
|
||||
/**
|
||||
* @brief Obtains polygon action type
|
||||
* @return Action type for current polygon
|
||||
*/
|
||||
ActionType getActionType() const;
|
||||
/**
|
||||
* @brief Obtains polygon enabled state
|
||||
* @return Whether polygon is enabled
|
||||
*/
|
||||
bool getEnabled() const;
|
||||
/**
|
||||
* @brief Obtains polygon maximum points to enter inside polygon causing no action
|
||||
* @return Maximum points to enter to current polygon and take no action
|
||||
*/
|
||||
int getMaxPoints() const;
|
||||
/**
|
||||
* @brief Obtains speed slowdown ratio for current polygon.
|
||||
* Applicable for SLOWDOWN model.
|
||||
* @return Speed slowdown ratio
|
||||
*/
|
||||
double getSlowdownRatio() const;
|
||||
/**
|
||||
* @brief Obtains required time before collision for current polygon.
|
||||
* Applicable for APPROACH model.
|
||||
* @return Time before collision in seconds
|
||||
*/
|
||||
double getTimeBeforeCollision() const;
|
||||
|
||||
/**
|
||||
* @brief Gets polygon points
|
||||
* @param poly Output polygon points (vertices)
|
||||
*/
|
||||
virtual void getPolygon(std::vector<Point> & poly) const;
|
||||
|
||||
/**
|
||||
* @brief Updates polygon from footprint subscriber (if any)
|
||||
*/
|
||||
void updatePolygon();
|
||||
|
||||
/**
|
||||
* @brief Gets number of points inside given polygon
|
||||
* @param points Input array of points to be checked
|
||||
* @return Number of points inside polygon. If there are no points,
|
||||
* returns zero value.
|
||||
*/
|
||||
virtual int getPointsInside(const std::vector<Point> & points) const;
|
||||
|
||||
/**
|
||||
* @brief Obtains estimated (simulated) time before a collision.
|
||||
* Applicable for APPROACH model.
|
||||
* @param collision_points Array of 2D obstacle points
|
||||
* @param velocity Simulated robot velocity
|
||||
* @return Estimated time before a collision. If there is no collision,
|
||||
* return value will be negative.
|
||||
*/
|
||||
double getCollisionTime(
|
||||
const std::vector<Point> & collision_points,
|
||||
const Velocity & velocity) const;
|
||||
|
||||
/**
|
||||
* @brief Publishes polygon message into a its own topic
|
||||
*/
|
||||
void publish() const;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Supporting routine obtaining ROS-parameters common for all shapes
|
||||
* @param polygon_pub_topic Output name of polygon publishing topic
|
||||
* @return True if all parameters were obtained or false in failure case
|
||||
*/
|
||||
bool getCommonParameters(std::string & polygon_pub_topic);
|
||||
|
||||
/**
|
||||
* @brief Supporting routine obtaining polygon-specific ROS-parameters
|
||||
* @param polygon_pub_topic Output name of polygon publishing topic
|
||||
* @param footprint_topic Output name of footprint topic. Empty, if no footprint subscription
|
||||
* @return True if all parameters were obtained or false in failure case
|
||||
*/
|
||||
virtual bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic);
|
||||
|
||||
/**
|
||||
* @brief Checks if point is inside polygon
|
||||
* @param point Given point to check
|
||||
* @return True if given point is inside polygon, otherwise false
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a parameter change is detected
|
||||
* @param event ParameterEvent message
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
|
||||
std::vector<rclcpp::Parameter> parameters);
|
||||
|
||||
bool isPointInside(const Point & point) const;
|
||||
|
||||
// ----- Variables -----
|
||||
|
||||
/// @brief Collision Monitor node
|
||||
nav2_util::LifecycleNode::WeakPtr node_;
|
||||
/// @brief Collision monitor node logger stored for further usage
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")};
|
||||
/// @brief Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
|
||||
// Basic parameters
|
||||
/// @brief Name of polygon
|
||||
std::string polygon_name_;
|
||||
/// @brief Action type for the polygon
|
||||
ActionType action_type_;
|
||||
/// @brief Maximum number of data readings within a zone to not trigger the action
|
||||
int max_points_;
|
||||
/// @brief Robot slowdown (share of its actual speed)
|
||||
double slowdown_ratio_;
|
||||
/// @brief Time before collision in seconds
|
||||
double time_before_collision_;
|
||||
/// @brief Time step for robot movement simulation
|
||||
double simulation_time_step_;
|
||||
/// @brief Footprint subscriber
|
||||
std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
|
||||
/// @brief Whether polygon is enabled
|
||||
bool enabled_;
|
||||
|
||||
// Global variables
|
||||
/// @brief TF buffer
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
/// @brief Base frame ID
|
||||
std::string base_frame_id_;
|
||||
/// @brief Transform tolerance
|
||||
tf2::Duration transform_tolerance_;
|
||||
|
||||
// Visualization
|
||||
/// @brief Whether to publish the polygon
|
||||
bool visualize_;
|
||||
/// @brief Polygon points stored for later publishing
|
||||
geometry_msgs::msg::Polygon polygon_;
|
||||
/// @brief Polygon publisher for visualization purposes
|
||||
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_;
|
||||
|
||||
/// @brief Polygon points (vertices)
|
||||
std::vector<Point> poly_;
|
||||
}; // class Polygon
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#endif // NAV2_COLLISION_MONITOR__POLYGON_HPP_
|
||||
@@ -0,0 +1,105 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_COLLISION_MONITOR__RANGE_HPP_
|
||||
#define NAV2_COLLISION_MONITOR__RANGE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include "sensor_msgs/msg/range.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/source.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Implementation for IR/ultrasound range sensor source
|
||||
*/
|
||||
class Range : public Source
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Range constructor
|
||||
* @param node Collision Monitor node pointer
|
||||
* @param source_name Name of data source
|
||||
* @param tf_buffer Shared pointer to a TF buffer
|
||||
* @param base_frame_id Robot base frame ID. The output data will be transformed into this frame.
|
||||
* @param global_frame_id Global frame ID for correct transform calculation
|
||||
* @param transform_tolerance Transform tolerance
|
||||
* @param source_timeout Maximum time interval in which data is considered valid
|
||||
* @param base_shift_correction Whether to correct source data towards to base frame movement,
|
||||
* considering the difference between current time and latest source time
|
||||
*/
|
||||
Range(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction);
|
||||
/**
|
||||
* @brief Range destructor
|
||||
*/
|
||||
~Range();
|
||||
|
||||
/**
|
||||
* @brief Data source configuration routine. Obtains ROS-parameters
|
||||
* and creates range sensor subscriber.
|
||||
*/
|
||||
void configure();
|
||||
|
||||
/**
|
||||
* @brief Adds latest data from range sensor to the data array.
|
||||
* @param curr_time Current node time for data interpolation
|
||||
* @param data Array where the data from source to be added.
|
||||
* Added data is transformed to base_frame_id_ coordinate system at curr_time.
|
||||
*/
|
||||
void getData(
|
||||
const rclcpp::Time & curr_time,
|
||||
std::vector<Point> & data) const;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Getting sensor-specific ROS-parameters
|
||||
* @param source_topic Output name of source subscription topic
|
||||
*/
|
||||
void getParameters(std::string & source_topic);
|
||||
|
||||
/**
|
||||
* @brief Range sensor data callback
|
||||
* @param msg Shared pointer to Range sensor message
|
||||
*/
|
||||
void dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg);
|
||||
|
||||
// ----- Variables -----
|
||||
|
||||
/// @brief Range sensor data subscriber
|
||||
rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr data_sub_;
|
||||
|
||||
/// @brief Angle increment (in rad) between two obstacle points at the range arc
|
||||
double obstacles_angle_;
|
||||
|
||||
/// @brief Latest data obtained from range sensor
|
||||
sensor_msgs::msg::Range::ConstSharedPtr data_;
|
||||
}; // class Range
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#endif // NAV2_COLLISION_MONITOR__RANGE_HPP_
|
||||
@@ -0,0 +1,96 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_COLLISION_MONITOR__SCAN_HPP_
|
||||
#define NAV2_COLLISION_MONITOR__SCAN_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||
#include "nav2_util/robot_utils.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/source.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Implementation for laser scanner source
|
||||
*/
|
||||
class Scan : public Source
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Scan constructor
|
||||
* @param node Collision Monitor node pointer
|
||||
* @param source_name Name of data source
|
||||
* @param tf_buffer Shared pointer to a TF buffer
|
||||
* @param base_frame_id Robot base frame ID. The output data will be transformed into this frame.
|
||||
* @param global_frame_id Global frame ID for correct transform calculation
|
||||
* @param transform_tolerance Transform tolerance
|
||||
* @param source_timeout Maximum time interval in which data is considered valid
|
||||
* @param base_shift_correction Whether to correct source data towards to base frame movement,
|
||||
* considering the difference between current time and latest source time
|
||||
*/
|
||||
Scan(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction);
|
||||
/**
|
||||
* @brief Scan destructor
|
||||
*/
|
||||
~Scan();
|
||||
|
||||
/**
|
||||
* @brief Data source configuration routine. Obtains ROS-parameters
|
||||
* and creates laser scanner subscriber.
|
||||
*/
|
||||
void configure();
|
||||
|
||||
/**
|
||||
* @brief Adds latest data from laser scanner to the data array.
|
||||
* @param curr_time Current node time for data interpolation
|
||||
* @param data Array where the data from source to be added.
|
||||
* Added data is transformed to base_frame_id_ coordinate system at curr_time.
|
||||
*/
|
||||
void getData(
|
||||
const rclcpp::Time & curr_time,
|
||||
std::vector<Point> & data) const;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Laser scanner data callback
|
||||
* @param msg Shared pointer to LaserScan message
|
||||
*/
|
||||
void dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg);
|
||||
|
||||
// ----- Variables -----
|
||||
|
||||
/// @brief Laser scanner data subscriber
|
||||
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr data_sub_;
|
||||
|
||||
/// @brief Latest data obtained from laser scanner
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr data_;
|
||||
}; // class Scan
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#endif // NAV2_COLLISION_MONITOR__SCAN_HPP_
|
||||
@@ -0,0 +1,146 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_COLLISION_MONITOR__SOURCE_HPP_
|
||||
#define NAV2_COLLISION_MONITOR__SOURCE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "tf2/time.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/types.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Basic data source class
|
||||
*/
|
||||
class Source
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Source constructor
|
||||
* @param node Collision Monitor node pointer
|
||||
* @param source_name Name of data source
|
||||
* @param tf_buffer Shared pointer to a TF buffer
|
||||
* @param base_frame_id Robot base frame ID. The output data will be transformed into this frame.
|
||||
* @param global_frame_id Global frame ID for correct transform calculation
|
||||
* @param transform_tolerance Transform tolerance
|
||||
* @param source_timeout Maximum time interval in which data is considered valid
|
||||
* @param base_shift_correction Whether to correct source data towards to base frame movement,
|
||||
* considering the difference between current time and latest source time
|
||||
*/
|
||||
Source(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction);
|
||||
/**
|
||||
* @brief Source destructor
|
||||
*/
|
||||
virtual ~Source();
|
||||
|
||||
/**
|
||||
* @brief Adds latest data from source to the data array.
|
||||
* Empty virtual method intended to be used in child implementations.
|
||||
* @param curr_time Current node time for data interpolation
|
||||
* @param data Array where the data from source to be added.
|
||||
* Added data is transformed to base_frame_id_ coordinate system at curr_time.
|
||||
*/
|
||||
virtual void getData(
|
||||
const rclcpp::Time & curr_time,
|
||||
std::vector<Point> & data) const = 0;
|
||||
|
||||
/**
|
||||
* @brief Obtains source enabled state
|
||||
* @return Whether source is enabled
|
||||
*/
|
||||
bool getEnabled() const;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Source configuration routine.
|
||||
* @return True in case of everything is configured correctly, or false otherwise
|
||||
*/
|
||||
bool configure();
|
||||
|
||||
/**
|
||||
* @brief Supporting routine obtaining ROS-parameters common for all data sources
|
||||
* @param source_topic Output name of source subscription topic
|
||||
*/
|
||||
void getCommonParameters(std::string & source_topic);
|
||||
|
||||
/**
|
||||
* @brief Checks whether the source data might be considered as valid
|
||||
* @param source_time Timestamp of latest obtained data
|
||||
* @param curr_time Current node time for source verification
|
||||
* @return True if data source is valid, otherwise false
|
||||
*/
|
||||
bool sourceValid(
|
||||
const rclcpp::Time & source_time,
|
||||
const rclcpp::Time & curr_time) const;
|
||||
|
||||
/**
|
||||
* @brief Callback executed when a parameter change is detected
|
||||
* @param event ParameterEvent message
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
|
||||
std::vector<rclcpp::Parameter> parameters);
|
||||
|
||||
// ----- Variables -----
|
||||
|
||||
/// @brief Collision Monitor node
|
||||
nav2_util::LifecycleNode::WeakPtr node_;
|
||||
/// @brief Collision monitor node logger stored for further usage
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")};
|
||||
/// @brief Dynamic parameters handler
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
|
||||
|
||||
// Basic parameters
|
||||
/// @brief Name of data source
|
||||
std::string source_name_;
|
||||
|
||||
// Global variables
|
||||
/// @brief TF buffer
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
/// @brief Robot base frame ID
|
||||
std::string base_frame_id_;
|
||||
/// @brief Global frame ID for correct transform calculation
|
||||
std::string global_frame_id_;
|
||||
/// @brief Transform tolerance
|
||||
tf2::Duration transform_tolerance_;
|
||||
/// @brief Maximum time interval in which data is considered valid
|
||||
rclcpp::Duration source_timeout_;
|
||||
/// @brief Whether to correct source data towards to base frame movement,
|
||||
/// considering the difference between current time and latest source time
|
||||
bool base_shift_correction_;
|
||||
/// @brief Whether source is enabled
|
||||
bool enabled_;
|
||||
}; // class Source
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#endif // NAV2_COLLISION_MONITOR__SOURCE_HPP_
|
||||
@@ -0,0 +1,80 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef NAV2_COLLISION_MONITOR__TYPES_HPP_
|
||||
#define NAV2_COLLISION_MONITOR__TYPES_HPP_
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
/// @brief Velocity for 2D model of motion
|
||||
struct Velocity
|
||||
{
|
||||
double x; // x-component of linear velocity
|
||||
double y; // y-component of linear velocity
|
||||
double tw; // z-component of angular twist
|
||||
|
||||
inline bool operator<(const Velocity & second) const
|
||||
{
|
||||
const double first_vel = x * x + y * y + tw * tw;
|
||||
const double second_vel = second.x * second.x + second.y * second.y + second.tw * second.tw;
|
||||
// This comparison includes rotations in place, where linear velocities are equal to zero
|
||||
return first_vel < second_vel;
|
||||
}
|
||||
|
||||
inline Velocity operator*(const double & mul) const
|
||||
{
|
||||
return {x * mul, y * mul, tw * mul};
|
||||
}
|
||||
|
||||
inline bool isZero() const
|
||||
{
|
||||
return x == 0.0 && y == 0.0 && tw == 0.0;
|
||||
}
|
||||
};
|
||||
|
||||
/// @brief 2D point
|
||||
struct Point
|
||||
{
|
||||
double x; // x-coordinate of point
|
||||
double y; // y-coordinate of point
|
||||
};
|
||||
|
||||
/// @brief 2D Pose
|
||||
struct Pose
|
||||
{
|
||||
double x; // x-coordinate of pose
|
||||
double y; // y-coordinate of pose
|
||||
double theta; // rotation angle of pose
|
||||
};
|
||||
|
||||
/// @brief Action type for robot
|
||||
enum ActionType
|
||||
{
|
||||
DO_NOTHING = 0, // No action
|
||||
STOP = 1, // Stop the robot
|
||||
SLOWDOWN = 2, // Slowdown in percentage from current operating speed
|
||||
APPROACH = 3 // Keep constant time interval before collision
|
||||
};
|
||||
|
||||
/// @brief Action for robot
|
||||
struct Action
|
||||
{
|
||||
ActionType action_type;
|
||||
Velocity req_vel;
|
||||
};
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#endif // NAV2_COLLISION_MONITOR__TYPES_HPP_
|
||||
@@ -0,0 +1,100 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.descriptions import ParameterFile
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Environment
|
||||
package_dir = get_package_share_directory('nav2_collision_monitor')
|
||||
|
||||
# Constant parameters
|
||||
lifecycle_nodes = ['collision_monitor']
|
||||
autostart = True
|
||||
|
||||
# Launch arguments
|
||||
# 1. Create the launch configuration variables
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
|
||||
# 2. Declare the launch arguments
|
||||
declare_namespace_cmd = DeclareLaunchArgument(
|
||||
'namespace',
|
||||
default_value='',
|
||||
description='Top-level namespace')
|
||||
|
||||
declare_use_sim_time_cmd = DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='True',
|
||||
description='Use simulation (Gazebo) clock if true')
|
||||
|
||||
declare_params_file_cmd = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'),
|
||||
description='Full path to the ROS2 parameters file to use for all launched nodes')
|
||||
|
||||
# Create our own temporary YAML files that include substitutions
|
||||
param_substitutions = {
|
||||
'use_sim_time': use_sim_time}
|
||||
|
||||
configured_params = ParameterFile(
|
||||
RewrittenYaml(
|
||||
source_file=params_file,
|
||||
root_key=namespace,
|
||||
param_rewrites=param_substitutions,
|
||||
convert_types=True),
|
||||
allow_substs=True)
|
||||
|
||||
# Nodes launching commands
|
||||
start_lifecycle_manager_cmd = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager',
|
||||
output='screen',
|
||||
emulate_tty=True, # https://github.com/ros2/launch/issues/188
|
||||
parameters=[{'use_sim_time': use_sim_time},
|
||||
{'autostart': autostart},
|
||||
{'node_names': lifecycle_nodes}])
|
||||
|
||||
start_collision_monitor_cmd = Node(
|
||||
package='nav2_collision_monitor',
|
||||
executable='collision_monitor',
|
||||
output='screen',
|
||||
emulate_tty=True, # https://github.com/ros2/launch/issues/188
|
||||
parameters=[configured_params])
|
||||
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Launch arguments
|
||||
ld.add_action(declare_namespace_cmd)
|
||||
ld.add_action(declare_use_sim_time_cmd)
|
||||
ld.add_action(declare_params_file_cmd)
|
||||
|
||||
# Node launching commands
|
||||
ld.add_action(start_lifecycle_manager_cmd)
|
||||
ld.add_action(start_collision_monitor_cmd)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,32 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>nav2_collision_monitor</name>
|
||||
<version>1.1.18</version>
|
||||
<description>Collision Monitor</description>
|
||||
<maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
|
||||
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav2_common</depend>
|
||||
<depend>nav2_util</depend>
|
||||
<depend>nav2_costmap_2d</depend>
|
||||
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,54 @@
|
||||
collision_monitor:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
base_frame_id: "base_footprint"
|
||||
odom_frame_id: "odom"
|
||||
cmd_vel_in_topic: "cmd_vel_raw"
|
||||
cmd_vel_out_topic: "cmd_vel"
|
||||
transform_tolerance: 0.5
|
||||
source_timeout: 5.0
|
||||
base_shift_correction: True
|
||||
stop_pub_timeout: 2.0
|
||||
# Polygons represent zone around the robot for "stop" and "slowdown" action types,
|
||||
# and robot footprint for "approach" action type.
|
||||
# Footprint could be "polygon" type with dynamically set footprint from footprint_topic
|
||||
# or "circle" type with static footprint set by radius. "footprint_topic" parameter
|
||||
# to be ignored in circular case.
|
||||
polygons: ["PolygonStop"]
|
||||
PolygonStop:
|
||||
type: "polygon"
|
||||
points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3]
|
||||
action_type: "stop"
|
||||
max_points: 3
|
||||
visualize: True
|
||||
polygon_pub_topic: "polygon_stop"
|
||||
enabled: True
|
||||
PolygonSlow:
|
||||
type: "polygon"
|
||||
points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4]
|
||||
action_type: "slowdown"
|
||||
max_points: 3
|
||||
slowdown_ratio: 0.3
|
||||
visualize: True
|
||||
polygon_pub_topic: "polygon_slowdown"
|
||||
enabled: True
|
||||
FootprintApproach:
|
||||
type: "polygon"
|
||||
action_type: "approach"
|
||||
footprint_topic: "/local_costmap/published_footprint"
|
||||
time_before_collision: 2.0
|
||||
simulation_time_step: 0.1
|
||||
max_points: 5
|
||||
visualize: False
|
||||
enabled: True
|
||||
observation_sources: ["scan"]
|
||||
scan:
|
||||
type: "scan"
|
||||
topic: "/scan"
|
||||
enabled: True
|
||||
pointcloud:
|
||||
type: "pointcloud"
|
||||
topic: "/intel_realsense_r200_depth/points"
|
||||
min_height: 0.1
|
||||
max_height: 0.5
|
||||
enabled: True
|
||||
@@ -0,0 +1,104 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "nav2_collision_monitor/circle.hpp"
|
||||
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
#include <exception>
|
||||
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
Circle::Circle(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & polygon_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const tf2::Duration & transform_tolerance)
|
||||
: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance)
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Creating Circle", polygon_name_.c_str());
|
||||
}
|
||||
|
||||
Circle::~Circle()
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Destroying Circle", polygon_name_.c_str());
|
||||
}
|
||||
|
||||
void Circle::getPolygon(std::vector<Point> & poly) const
|
||||
{
|
||||
// Number of polygon points. More edges means better approximation.
|
||||
const double polygon_edges = 16;
|
||||
// Increment of angle during points position calculation
|
||||
double angle_increment = 2 * M_PI / polygon_edges;
|
||||
|
||||
// Clear polygon before filling
|
||||
poly.clear();
|
||||
|
||||
// Making new polygon looks like a circle
|
||||
Point p;
|
||||
for (double angle = 0.0; angle < 2 * M_PI; angle += angle_increment) {
|
||||
p.x = radius_ * std::cos(angle);
|
||||
p.y = radius_ * std::sin(angle);
|
||||
poly.push_back(p);
|
||||
}
|
||||
}
|
||||
|
||||
int Circle::getPointsInside(const std::vector<Point> & points) const
|
||||
{
|
||||
int num = 0;
|
||||
for (Point point : points) {
|
||||
if (point.x * point.x + point.y * point.y < radius_squared_) {
|
||||
num++;
|
||||
}
|
||||
}
|
||||
|
||||
return num;
|
||||
}
|
||||
|
||||
bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic)
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
if (!getCommonParameters(polygon_pub_topic)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// There is no footprint subscription for the Circle. Thus, set string as empty.
|
||||
footprint_topic.clear();
|
||||
|
||||
try {
|
||||
// Leave it not initialized: the will cause an error if it will not set
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE);
|
||||
radius_ = node->get_parameter(polygon_name_ + ".radius").as_double();
|
||||
radius_squared_ = radius_ * radius_;
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(
|
||||
logger_,
|
||||
"[%s]: Error while getting circle parameters: %s",
|
||||
polygon_name_.c_str(), ex.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
@@ -0,0 +1,502 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "nav2_collision_monitor/collision_monitor_node.hpp"
|
||||
|
||||
#include <exception>
|
||||
#include <utility>
|
||||
#include <functional>
|
||||
|
||||
#include "tf2_ros/create_timer_ros.h"
|
||||
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/kinematics.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions & options)
|
||||
: nav2_util::LifecycleNode("collision_monitor", "", options),
|
||||
process_active_(false), robot_action_prev_{DO_NOTHING, {-1.0, -1.0, -1.0}},
|
||||
stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0)
|
||||
{
|
||||
}
|
||||
|
||||
CollisionMonitor::~CollisionMonitor()
|
||||
{
|
||||
polygons_.clear();
|
||||
sources_.clear();
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Configuring");
|
||||
|
||||
// Transform buffer and listener initialization
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
|
||||
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
this->get_node_base_interface(),
|
||||
this->get_node_timers_interface());
|
||||
tf_buffer_->setCreateTimerInterface(timer_interface);
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
|
||||
std::string cmd_vel_in_topic;
|
||||
std::string cmd_vel_out_topic;
|
||||
|
||||
// Obtaining ROS parameters
|
||||
if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) {
|
||||
return nav2_util::CallbackReturn::FAILURE;
|
||||
}
|
||||
|
||||
cmd_vel_in_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||
cmd_vel_in_topic, 1,
|
||||
std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1));
|
||||
cmd_vel_out_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
|
||||
cmd_vel_out_topic, 1);
|
||||
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Activating");
|
||||
|
||||
// Activating lifecycle publisher
|
||||
cmd_vel_out_pub_->on_activate();
|
||||
|
||||
// Activating polygons
|
||||
for (std::shared_ptr<Polygon> polygon : polygons_) {
|
||||
polygon->activate();
|
||||
}
|
||||
|
||||
// Since polygons are being published when cmd_vel_in appears,
|
||||
// we need to publish polygons first time to display them at startup
|
||||
publishPolygons();
|
||||
|
||||
// Activating main worker
|
||||
process_active_ = true;
|
||||
|
||||
// Creating bond connection
|
||||
createBond();
|
||||
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Deactivating");
|
||||
|
||||
// Deactivating main worker
|
||||
process_active_ = false;
|
||||
|
||||
// Reset action type to default after worker deactivating
|
||||
robot_action_prev_ = {DO_NOTHING, {-1.0, -1.0, -1.0}};
|
||||
|
||||
// Deactivating polygons
|
||||
for (std::shared_ptr<Polygon> polygon : polygons_) {
|
||||
polygon->deactivate();
|
||||
}
|
||||
|
||||
// Deactivating lifecycle publishers
|
||||
cmd_vel_out_pub_->on_deactivate();
|
||||
|
||||
// Destroying bond connection
|
||||
destroyBond();
|
||||
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Cleaning up");
|
||||
|
||||
cmd_vel_in_sub_.reset();
|
||||
cmd_vel_out_pub_.reset();
|
||||
|
||||
polygons_.clear();
|
||||
sources_.clear();
|
||||
|
||||
tf_listener_.reset();
|
||||
tf_buffer_.reset();
|
||||
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
nav2_util::CallbackReturn
|
||||
CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Shutting down");
|
||||
|
||||
return nav2_util::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg)
|
||||
{
|
||||
// If message contains NaN or Inf, ignore
|
||||
if (!nav2_util::validateTwist(*msg)) {
|
||||
RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
|
||||
return;
|
||||
}
|
||||
|
||||
process({msg->linear.x, msg->linear.y, msg->angular.z});
|
||||
}
|
||||
|
||||
void CollisionMonitor::publishVelocity(const Action & robot_action)
|
||||
{
|
||||
if (robot_action.req_vel.isZero()) {
|
||||
if (!robot_action_prev_.req_vel.isZero()) {
|
||||
// Robot just stopped: saving stop timestamp and continue
|
||||
stop_stamp_ = this->now();
|
||||
} else if (this->now() - stop_stamp_ > stop_pub_timeout_) {
|
||||
// More than stop_pub_timeout_ passed after robot has been stopped.
|
||||
// Cease publishing output cmd_vel.
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
std::unique_ptr<geometry_msgs::msg::Twist> cmd_vel_out_msg =
|
||||
std::make_unique<geometry_msgs::msg::Twist>();
|
||||
cmd_vel_out_msg->linear.x = robot_action.req_vel.x;
|
||||
cmd_vel_out_msg->linear.y = robot_action.req_vel.y;
|
||||
cmd_vel_out_msg->angular.z = robot_action.req_vel.tw;
|
||||
// linear.z, angular.x and angular.y will remain 0.0
|
||||
|
||||
cmd_vel_out_pub_->publish(std::move(cmd_vel_out_msg));
|
||||
}
|
||||
|
||||
bool CollisionMonitor::getParameters(
|
||||
std::string & cmd_vel_in_topic,
|
||||
std::string & cmd_vel_out_topic)
|
||||
{
|
||||
std::string base_frame_id, odom_frame_id;
|
||||
tf2::Duration transform_tolerance;
|
||||
rclcpp::Duration source_timeout(2.0, 0.0);
|
||||
|
||||
auto node = shared_from_this();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "cmd_vel_in_topic", rclcpp::ParameterValue("cmd_vel_raw"));
|
||||
cmd_vel_in_topic = get_parameter("cmd_vel_in_topic").as_string();
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "cmd_vel_out_topic", rclcpp::ParameterValue("cmd_vel"));
|
||||
cmd_vel_out_topic = get_parameter("cmd_vel_out_topic").as_string();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "base_frame_id", rclcpp::ParameterValue("base_footprint"));
|
||||
base_frame_id = get_parameter("base_frame_id").as_string();
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "odom_frame_id", rclcpp::ParameterValue("odom"));
|
||||
odom_frame_id = get_parameter("odom_frame_id").as_string();
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "transform_tolerance", rclcpp::ParameterValue(0.1));
|
||||
transform_tolerance =
|
||||
tf2::durationFromSec(get_parameter("transform_tolerance").as_double());
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "source_timeout", rclcpp::ParameterValue(2.0));
|
||||
source_timeout =
|
||||
rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double());
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "base_shift_correction", rclcpp::ParameterValue(true));
|
||||
const bool base_shift_correction =
|
||||
get_parameter("base_shift_correction").as_bool();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "stop_pub_timeout", rclcpp::ParameterValue(1.0));
|
||||
stop_pub_timeout_ =
|
||||
rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double());
|
||||
|
||||
if (!configurePolygons(base_frame_id, transform_tolerance)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (
|
||||
!configureSources(
|
||||
base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CollisionMonitor::configurePolygons(
|
||||
const std::string & base_frame_id,
|
||||
const tf2::Duration & transform_tolerance)
|
||||
{
|
||||
try {
|
||||
auto node = shared_from_this();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "polygons", rclcpp::ParameterValue(std::vector<std::string>()));
|
||||
std::vector<std::string> polygon_names = get_parameter("polygons").as_string_array();
|
||||
for (std::string polygon_name : polygon_names) {
|
||||
// Leave it not initialized: the will cause an error if it will not set
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name + ".type", rclcpp::PARAMETER_STRING);
|
||||
const std::string polygon_type = get_parameter(polygon_name + ".type").as_string();
|
||||
|
||||
if (polygon_type == "polygon") {
|
||||
polygons_.push_back(
|
||||
std::make_shared<Polygon>(
|
||||
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
|
||||
} else if (polygon_type == "circle") {
|
||||
polygons_.push_back(
|
||||
std::make_shared<Circle>(
|
||||
node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
|
||||
} else { // Error if something else
|
||||
RCLCPP_ERROR(
|
||||
get_logger(),
|
||||
"[%s]: Unknown polygon type: %s",
|
||||
polygon_name.c_str(), polygon_type.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// Configure last added polygon
|
||||
if (!polygons_.back()->configure()) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CollisionMonitor::configureSources(
|
||||
const std::string & base_frame_id,
|
||||
const std::string & odom_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction)
|
||||
{
|
||||
try {
|
||||
auto node = shared_from_this();
|
||||
|
||||
// Leave it to be not initialized: to intentionally cause an error if it will not set
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY);
|
||||
std::vector<std::string> source_names = get_parameter("observation_sources").as_string_array();
|
||||
for (std::string source_name : source_names) {
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, source_name + ".type",
|
||||
rclcpp::ParameterValue("scan")); // Laser scanner by default
|
||||
const std::string source_type = get_parameter(source_name + ".type").as_string();
|
||||
|
||||
if (source_type == "scan") {
|
||||
std::shared_ptr<Scan> s = std::make_shared<Scan>(
|
||||
node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
|
||||
transform_tolerance, source_timeout, base_shift_correction);
|
||||
|
||||
s->configure();
|
||||
|
||||
sources_.push_back(s);
|
||||
} else if (source_type == "pointcloud") {
|
||||
std::shared_ptr<PointCloud> p = std::make_shared<PointCloud>(
|
||||
node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
|
||||
transform_tolerance, source_timeout, base_shift_correction);
|
||||
|
||||
p->configure();
|
||||
|
||||
sources_.push_back(p);
|
||||
} else if (source_type == "range") {
|
||||
std::shared_ptr<Range> r = std::make_shared<Range>(
|
||||
node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
|
||||
transform_tolerance, source_timeout, base_shift_correction);
|
||||
|
||||
r->configure();
|
||||
|
||||
sources_.push_back(r);
|
||||
} else { // Error if something else
|
||||
RCLCPP_ERROR(
|
||||
get_logger(),
|
||||
"[%s]: Unknown source type: %s",
|
||||
source_name.c_str(), source_type.c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void CollisionMonitor::process(const Velocity & cmd_vel_in)
|
||||
{
|
||||
// Current timestamp for all inner routines prolongation
|
||||
rclcpp::Time curr_time = this->now();
|
||||
|
||||
// Do nothing if main worker in non-active state
|
||||
if (!process_active_) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Points array collected from different data sources in a robot base frame
|
||||
std::vector<Point> collision_points;
|
||||
|
||||
// Fill collision_points array from different data sources
|
||||
for (std::shared_ptr<Source> source : sources_) {
|
||||
if (source->getEnabled()) {
|
||||
source->getData(curr_time, collision_points);
|
||||
}
|
||||
}
|
||||
|
||||
// By default - there is no action
|
||||
Action robot_action{DO_NOTHING, cmd_vel_in};
|
||||
// Polygon causing robot action (if any)
|
||||
std::shared_ptr<Polygon> action_polygon;
|
||||
|
||||
for (std::shared_ptr<Polygon> polygon : polygons_) {
|
||||
if (!polygon->getEnabled()) {
|
||||
continue;
|
||||
}
|
||||
if (robot_action.action_type == STOP) {
|
||||
// If robot already should stop, do nothing
|
||||
break;
|
||||
}
|
||||
|
||||
const ActionType at = polygon->getActionType();
|
||||
if (at == STOP || at == SLOWDOWN) {
|
||||
// Process STOP/SLOWDOWN for the selected polygon
|
||||
if (processStopSlowdown(polygon, collision_points, cmd_vel_in, robot_action)) {
|
||||
action_polygon = polygon;
|
||||
}
|
||||
} else if (at == APPROACH) {
|
||||
// Process APPROACH for the selected polygon
|
||||
if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) {
|
||||
action_polygon = polygon;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (robot_action.action_type != robot_action_prev_.action_type) {
|
||||
// Report changed robot behavior
|
||||
printAction(robot_action, action_polygon);
|
||||
}
|
||||
|
||||
// Publish required robot velocity
|
||||
publishVelocity(robot_action);
|
||||
|
||||
// Publish polygons for better visualization
|
||||
publishPolygons();
|
||||
|
||||
robot_action_prev_ = robot_action;
|
||||
}
|
||||
|
||||
bool CollisionMonitor::processStopSlowdown(
|
||||
const std::shared_ptr<Polygon> polygon,
|
||||
const std::vector<Point> & collision_points,
|
||||
const Velocity & velocity,
|
||||
Action & robot_action) const
|
||||
{
|
||||
if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) {
|
||||
if (polygon->getActionType() == STOP) {
|
||||
// Setting up zero velocity for STOP model
|
||||
robot_action.action_type = STOP;
|
||||
robot_action.req_vel.x = 0.0;
|
||||
robot_action.req_vel.y = 0.0;
|
||||
robot_action.req_vel.tw = 0.0;
|
||||
return true;
|
||||
} else { // SLOWDOWN
|
||||
const Velocity safe_vel = velocity * polygon->getSlowdownRatio();
|
||||
// Check that currently calculated velocity is safer than
|
||||
// chosen for previous shapes one
|
||||
if (safe_vel < robot_action.req_vel) {
|
||||
robot_action.action_type = SLOWDOWN;
|
||||
robot_action.req_vel = safe_vel;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool CollisionMonitor::processApproach(
|
||||
const std::shared_ptr<Polygon> polygon,
|
||||
const std::vector<Point> & collision_points,
|
||||
const Velocity & velocity,
|
||||
Action & robot_action) const
|
||||
{
|
||||
polygon->updatePolygon();
|
||||
|
||||
// Obtain time before a collision
|
||||
const double collision_time = polygon->getCollisionTime(collision_points, velocity);
|
||||
if (collision_time >= 0.0) {
|
||||
// If collision will occurr, reduce robot speed
|
||||
const double change_ratio = collision_time / polygon->getTimeBeforeCollision();
|
||||
const Velocity safe_vel = velocity * change_ratio;
|
||||
// Check that currently calculated velocity is safer than
|
||||
// chosen for previous shapes one
|
||||
if (safe_vel < robot_action.req_vel) {
|
||||
robot_action.action_type = APPROACH;
|
||||
robot_action.req_vel = safe_vel;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void CollisionMonitor::printAction(
|
||||
const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const
|
||||
{
|
||||
if (robot_action.action_type == STOP) {
|
||||
RCLCPP_INFO(
|
||||
get_logger(),
|
||||
"Robot to stop due to %s polygon",
|
||||
action_polygon->getName().c_str());
|
||||
} else if (robot_action.action_type == SLOWDOWN) {
|
||||
RCLCPP_INFO(
|
||||
get_logger(),
|
||||
"Robot to slowdown for %f percents due to %s polygon",
|
||||
action_polygon->getSlowdownRatio() * 100,
|
||||
action_polygon->getName().c_str());
|
||||
} else if (robot_action.action_type == APPROACH) {
|
||||
RCLCPP_INFO(
|
||||
get_logger(),
|
||||
"Robot to approach for %f seconds away from collision",
|
||||
action_polygon->getTimeBeforeCollision());
|
||||
} else { // robot_action.action_type == DO_NOTHING
|
||||
RCLCPP_INFO(
|
||||
get_logger(),
|
||||
"Robot to continue normal operation");
|
||||
}
|
||||
}
|
||||
|
||||
void CollisionMonitor::publishPolygons() const
|
||||
{
|
||||
for (std::shared_ptr<Polygon> polygon : polygons_) {
|
||||
if (polygon->getEnabled()) {
|
||||
polygon->publish();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// Register the component with class_loader.
|
||||
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||
// is being loaded into a running process.
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(nav2_collision_monitor::CollisionMonitor)
|
||||
@@ -0,0 +1,70 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "nav2_collision_monitor/kinematics.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
void transformPoints(const Pose & pose, std::vector<Point> & points)
|
||||
{
|
||||
const double cos_theta = std::cos(pose.theta);
|
||||
const double sin_theta = std::sin(pose.theta);
|
||||
|
||||
for (Point & point : points) {
|
||||
// p = R*p' + pose
|
||||
// p' = Rt * (p - pose)
|
||||
// where:
|
||||
// p - point coordinates in initial frame
|
||||
// p' - point coordinates in a new frame
|
||||
// R - rotation matrix =
|
||||
// [cos_theta -sin_theta]
|
||||
// [sin_theta cos_theta]
|
||||
// Rt - transposed (inverted) rotation matrix
|
||||
const double mul_x = point.x - pose.x;
|
||||
const double mul_y = point.y - pose.y;
|
||||
point.x = mul_x * cos_theta + mul_y * sin_theta;
|
||||
point.y = -mul_x * sin_theta + mul_y * cos_theta;
|
||||
}
|
||||
}
|
||||
|
||||
void projectState(const double & dt, Pose & pose, Velocity & velocity)
|
||||
{
|
||||
const double theta = velocity.tw * dt;
|
||||
const double cos_theta = std::cos(theta);
|
||||
const double sin_theta = std::sin(theta);
|
||||
|
||||
// p' = p + vel*dt
|
||||
// where:
|
||||
// p - initial pose
|
||||
// p' - projected pose
|
||||
pose.x = pose.x + velocity.x * dt;
|
||||
pose.y = pose.y + velocity.y * dt;
|
||||
// Rotate the pose on theta
|
||||
pose.theta = pose.theta + theta;
|
||||
|
||||
// vel' = R*vel
|
||||
// where:
|
||||
// vel - initial velocity
|
||||
// R - rotation matrix
|
||||
// vel' - rotated velocity
|
||||
const double velocity_upd_x = velocity.x * cos_theta - velocity.y * sin_theta;
|
||||
const double velocity_upd_y = velocity.x * sin_theta + velocity.y * cos_theta;
|
||||
velocity.x = velocity_upd_x;
|
||||
velocity.y = velocity_upd_y;
|
||||
}
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
@@ -0,0 +1,29 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/collision_monitor_node.hpp"
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<nav2_collision_monitor::CollisionMonitor>();
|
||||
rclcpp::spin(node->get_node_base_interface());
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,144 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "nav2_collision_monitor/pointcloud.hpp"
|
||||
|
||||
#include <functional>
|
||||
|
||||
#include "sensor_msgs/point_cloud2_iterator.hpp"
|
||||
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
PointCloud::PointCloud(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction)
|
||||
: Source(
|
||||
node, source_name, tf_buffer, base_frame_id, global_frame_id,
|
||||
transform_tolerance, source_timeout, base_shift_correction),
|
||||
data_(nullptr)
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str());
|
||||
}
|
||||
|
||||
PointCloud::~PointCloud()
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Destroying PointCloud", source_name_.c_str());
|
||||
data_sub_.reset();
|
||||
}
|
||||
|
||||
void PointCloud::configure()
|
||||
{
|
||||
Source::configure();
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
std::string source_topic;
|
||||
|
||||
getParameters(source_topic);
|
||||
|
||||
rclcpp::QoS pointcloud_qos = rclcpp::SensorDataQoS(); // set to default
|
||||
data_sub_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
source_topic, pointcloud_qos,
|
||||
std::bind(&PointCloud::dataCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void PointCloud::getData(
|
||||
const rclcpp::Time & curr_time,
|
||||
std::vector<Point> & data) const
|
||||
{
|
||||
// Ignore data from the source if it is not being published yet or
|
||||
// not published for a long time
|
||||
if (data_ == nullptr) {
|
||||
return;
|
||||
}
|
||||
if (!sourceValid(data_->header.stamp, curr_time)) {
|
||||
return;
|
||||
}
|
||||
|
||||
tf2::Transform tf_transform;
|
||||
if (base_shift_correction_) {
|
||||
// Obtaining the transform to get data from source frame and time where it was received
|
||||
// to the base frame and current time
|
||||
if (
|
||||
!nav2_util::getTransform(
|
||||
data_->header.frame_id, data_->header.stamp,
|
||||
base_frame_id_, curr_time, global_frame_id_,
|
||||
transform_tolerance_, tf_buffer_, tf_transform))
|
||||
{
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// Obtaining the transform to get data from source frame to base frame without time shift
|
||||
// considered. Less accurate but much more faster option not dependent on state estimation
|
||||
// frames.
|
||||
if (
|
||||
!nav2_util::getTransform(
|
||||
data_->header.frame_id, base_frame_id_,
|
||||
transform_tolerance_, tf_buffer_, tf_transform))
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*data_, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*data_, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*data_, "z");
|
||||
|
||||
// Refill data array with PointCloud points in base frame
|
||||
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
|
||||
// Transform point coordinates from source frame -> to base frame
|
||||
tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z);
|
||||
tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
|
||||
|
||||
// Refill data array
|
||||
if (p_v3_b.z() >= min_height_ && p_v3_b.z() <= max_height_) {
|
||||
data.push_back({p_v3_b.x(), p_v3_b.y()});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PointCloud::getParameters(std::string & source_topic)
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
getCommonParameters(source_topic);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, source_name_ + ".min_height", rclcpp::ParameterValue(0.05));
|
||||
min_height_ = node->get_parameter(source_name_ + ".min_height").as_double();
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, source_name_ + ".max_height", rclcpp::ParameterValue(0.5));
|
||||
max_height_ = node->get_parameter(source_name_ + ".max_height").as_double();
|
||||
}
|
||||
|
||||
void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
|
||||
{
|
||||
data_ = msg;
|
||||
}
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
@@ -0,0 +1,419 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "nav2_collision_monitor/polygon.hpp"
|
||||
|
||||
#include <exception>
|
||||
#include <utility>
|
||||
|
||||
#include "geometry_msgs/msg/point.hpp"
|
||||
#include "geometry_msgs/msg/point32.hpp"
|
||||
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/kinematics.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
Polygon::Polygon(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & polygon_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const tf2::Duration & transform_tolerance)
|
||||
: node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING),
|
||||
slowdown_ratio_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer),
|
||||
base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance)
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str());
|
||||
}
|
||||
|
||||
Polygon::~Polygon()
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str());
|
||||
poly_.clear();
|
||||
dyn_params_handler_.reset();
|
||||
}
|
||||
|
||||
bool Polygon::configure()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
std::string polygon_pub_topic, footprint_topic;
|
||||
|
||||
if (!getParameters(polygon_pub_topic, footprint_topic)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!footprint_topic.empty()) {
|
||||
footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
|
||||
node, footprint_topic, *tf_buffer_,
|
||||
base_frame_id_, tf2::durationToSec(transform_tolerance_));
|
||||
}
|
||||
|
||||
if (visualize_) {
|
||||
// Fill polygon_ points for future usage
|
||||
std::vector<Point> poly;
|
||||
getPolygon(poly);
|
||||
for (const Point & p : poly) {
|
||||
geometry_msgs::msg::Point32 p_s;
|
||||
p_s.x = p.x;
|
||||
p_s.y = p.y;
|
||||
// p_s.z will remain 0.0
|
||||
polygon_.points.push_back(p_s);
|
||||
}
|
||||
|
||||
rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default
|
||||
polygon_pub_ = node->create_publisher<geometry_msgs::msg::PolygonStamped>(
|
||||
polygon_pub_topic, polygon_qos);
|
||||
}
|
||||
|
||||
// Add callback for dynamic parameters
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&Polygon::dynamicParametersCallback, this, std::placeholders::_1));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Polygon::activate()
|
||||
{
|
||||
if (visualize_) {
|
||||
polygon_pub_->on_activate();
|
||||
}
|
||||
}
|
||||
|
||||
void Polygon::deactivate()
|
||||
{
|
||||
if (visualize_) {
|
||||
polygon_pub_->on_deactivate();
|
||||
}
|
||||
}
|
||||
|
||||
std::string Polygon::getName() const
|
||||
{
|
||||
return polygon_name_;
|
||||
}
|
||||
|
||||
ActionType Polygon::getActionType() const
|
||||
{
|
||||
return action_type_;
|
||||
}
|
||||
|
||||
bool Polygon::getEnabled() const
|
||||
{
|
||||
return enabled_;
|
||||
}
|
||||
|
||||
int Polygon::getMaxPoints() const
|
||||
{
|
||||
return max_points_;
|
||||
}
|
||||
|
||||
double Polygon::getSlowdownRatio() const
|
||||
{
|
||||
return slowdown_ratio_;
|
||||
}
|
||||
|
||||
double Polygon::getTimeBeforeCollision() const
|
||||
{
|
||||
return time_before_collision_;
|
||||
}
|
||||
|
||||
void Polygon::getPolygon(std::vector<Point> & poly) const
|
||||
{
|
||||
poly = poly_;
|
||||
}
|
||||
|
||||
void Polygon::updatePolygon()
|
||||
{
|
||||
if (footprint_sub_ != nullptr) {
|
||||
// Get latest robot footprint from footprint subscriber
|
||||
std::vector<geometry_msgs::msg::Point> footprint_vec;
|
||||
std_msgs::msg::Header footprint_header;
|
||||
footprint_sub_->getFootprintInRobotFrame(footprint_vec, footprint_header);
|
||||
|
||||
std::size_t new_size = footprint_vec.size();
|
||||
poly_.resize(new_size);
|
||||
polygon_.points.resize(new_size);
|
||||
|
||||
geometry_msgs::msg::Point32 p_s;
|
||||
for (std::size_t i = 0; i < new_size; i++) {
|
||||
poly_[i] = {footprint_vec[i].x, footprint_vec[i].y};
|
||||
p_s.x = footprint_vec[i].x;
|
||||
p_s.y = footprint_vec[i].y;
|
||||
polygon_.points[i] = p_s;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int Polygon::getPointsInside(const std::vector<Point> & points) const
|
||||
{
|
||||
int num = 0;
|
||||
for (const Point & point : points) {
|
||||
if (isPointInside(point)) {
|
||||
num++;
|
||||
}
|
||||
}
|
||||
return num;
|
||||
}
|
||||
|
||||
double Polygon::getCollisionTime(
|
||||
const std::vector<Point> & collision_points,
|
||||
const Velocity & velocity) const
|
||||
{
|
||||
// Initial robot pose is {0,0} in base_footprint coordinates
|
||||
Pose pose = {0.0, 0.0, 0.0};
|
||||
Velocity vel = velocity;
|
||||
|
||||
// Array of points transformed to the frame concerned with pose on each simulation step
|
||||
std::vector<Point> points_transformed = collision_points;
|
||||
|
||||
// Check static polygon
|
||||
if (getPointsInside(points_transformed) >= max_points_) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// Robot movement simulation
|
||||
for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) {
|
||||
// Shift the robot pose towards to the vel during simulation_time_step_ time interval
|
||||
// NOTE: vel is changing during the simulation
|
||||
projectState(simulation_time_step_, pose, vel);
|
||||
// Transform collision_points to the frame concerned with current robot pose
|
||||
points_transformed = collision_points;
|
||||
transformPoints(pose, points_transformed);
|
||||
// If the collision occurred on this stage, return the actual time before a collision
|
||||
// as if robot was moved with given velocity
|
||||
if (getPointsInside(points_transformed) > max_points_) {
|
||||
return time;
|
||||
}
|
||||
}
|
||||
|
||||
// There is no collision
|
||||
return -1.0;
|
||||
}
|
||||
|
||||
void Polygon::publish() const
|
||||
{
|
||||
if (!visualize_) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
// Fill PolygonStamped struct
|
||||
std::unique_ptr<geometry_msgs::msg::PolygonStamped> poly_s =
|
||||
std::make_unique<geometry_msgs::msg::PolygonStamped>();
|
||||
poly_s->header.stamp = node->now();
|
||||
poly_s->header.frame_id = base_frame_id_;
|
||||
poly_s->polygon = polygon_;
|
||||
|
||||
// Publish polygon
|
||||
polygon_pub_->publish(std::move(poly_s));
|
||||
}
|
||||
|
||||
bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
try {
|
||||
// Get action type.
|
||||
// Leave it not initialized: the will cause an error if it will not set.
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".action_type", rclcpp::PARAMETER_STRING);
|
||||
const std::string at_str =
|
||||
node->get_parameter(polygon_name_ + ".action_type").as_string();
|
||||
if (at_str == "stop") {
|
||||
action_type_ = STOP;
|
||||
} else if (at_str == "slowdown") {
|
||||
action_type_ = SLOWDOWN;
|
||||
} else if (at_str == "approach") {
|
||||
action_type_ = APPROACH;
|
||||
} else { // Error if something else
|
||||
RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true));
|
||||
enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".max_points", rclcpp::ParameterValue(3));
|
||||
max_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int();
|
||||
|
||||
if (action_type_ == SLOWDOWN) {
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".slowdown_ratio", rclcpp::ParameterValue(0.5));
|
||||
slowdown_ratio_ = node->get_parameter(polygon_name_ + ".slowdown_ratio").as_double();
|
||||
}
|
||||
|
||||
if (action_type_ == APPROACH) {
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".time_before_collision", rclcpp::ParameterValue(2.0));
|
||||
time_before_collision_ =
|
||||
node->get_parameter(polygon_name_ + ".time_before_collision").as_double();
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".simulation_time_step", rclcpp::ParameterValue(0.1));
|
||||
simulation_time_step_ =
|
||||
node->get_parameter(polygon_name_ + ".simulation_time_step").as_double();
|
||||
}
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".visualize", rclcpp::ParameterValue(false));
|
||||
visualize_ = node->get_parameter(polygon_name_ + ".visualize").as_bool();
|
||||
if (visualize_) {
|
||||
// Get polygon topic parameter in case if it is going to be published
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name_));
|
||||
polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string();
|
||||
}
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(
|
||||
logger_,
|
||||
"[%s]: Error while getting common polygon parameters: %s",
|
||||
polygon_name_.c_str(), ex.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic)
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
if (!getCommonParameters(polygon_pub_topic)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
try {
|
||||
if (action_type_ == APPROACH) {
|
||||
// Obtain the footprint topic to make a footprint subscription for approach polygon
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".footprint_topic",
|
||||
rclcpp::ParameterValue("local_costmap/published_footprint"));
|
||||
footprint_topic =
|
||||
node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
|
||||
|
||||
// This is robot footprint: do not need to get polygon points from ROS parameters.
|
||||
// It will be set dynamically later.
|
||||
return true;
|
||||
} else {
|
||||
// Make it empty otherwise
|
||||
footprint_topic.clear();
|
||||
}
|
||||
|
||||
// Leave it not initialized: the will cause an error if it will not set
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY);
|
||||
std::vector<double> poly_row =
|
||||
node->get_parameter(polygon_name_ + ".points").as_double_array();
|
||||
// Check for points format correctness
|
||||
if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) {
|
||||
RCLCPP_ERROR(
|
||||
logger_,
|
||||
"[%s]: Polygon has incorrect points description",
|
||||
polygon_name_.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// Obtain polygon vertices
|
||||
Point point;
|
||||
bool first = true;
|
||||
for (double val : poly_row) {
|
||||
if (first) {
|
||||
point.x = val;
|
||||
} else {
|
||||
point.y = val;
|
||||
poly_.push_back(point);
|
||||
}
|
||||
first = !first;
|
||||
}
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(
|
||||
logger_,
|
||||
"[%s]: Error while getting polygon parameters: %s",
|
||||
polygon_name_.c_str(), ex.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
Polygon::dynamicParametersCallback(
|
||||
std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
|
||||
for (auto parameter : parameters) {
|
||||
const auto & param_type = parameter.get_type();
|
||||
const auto & param_name = parameter.get_name();
|
||||
|
||||
if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
|
||||
if (param_name == polygon_name_ + "." + "enabled") {
|
||||
enabled_ = parameter.as_bool();
|
||||
}
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
inline bool Polygon::isPointInside(const Point & point) const
|
||||
{
|
||||
// Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon."
|
||||
// Communications of the ACM 5.8 (1962): 434.
|
||||
// Implementation of ray crossings algorithm for point in polygon task solving.
|
||||
// Y coordinate is fixed. Moving the ray on X+ axis starting from given point.
|
||||
// Odd number of intersections with polygon boundaries means the point is inside polygon.
|
||||
const int poly_size = poly_.size();
|
||||
int i, j; // Polygon vertex iterators
|
||||
bool res = false; // Final result, initialized with already inverted value
|
||||
|
||||
// Starting from the edge where the last point of polygon is connected to the first
|
||||
i = poly_size - 1;
|
||||
for (j = 0; j < poly_size; j++) {
|
||||
// Checking the edge only if given point is between edge boundaries by Y coordinates.
|
||||
// One of the condition should contain equality in order to exclude the edges
|
||||
// parallel to X+ ray.
|
||||
if ((point.y <= poly_[i].y) == (point.y > poly_[j].y)) {
|
||||
// Calculating the intersection coordinate of X+ ray
|
||||
const double x_inter = poly_[i].x +
|
||||
(point.y - poly_[i].y) * (poly_[j].x - poly_[i].x) /
|
||||
(poly_[j].y - poly_[i].y);
|
||||
// If intersection with checked edge is greater than point.x coordinate, inverting the result
|
||||
if (x_inter > point.x) {
|
||||
res = !res;
|
||||
}
|
||||
}
|
||||
i = j;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
@@ -0,0 +1,165 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "nav2_collision_monitor/range.hpp"
|
||||
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
Range::Range(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction)
|
||||
: Source(
|
||||
node, source_name, tf_buffer, base_frame_id, global_frame_id,
|
||||
transform_tolerance, source_timeout, base_shift_correction),
|
||||
data_(nullptr)
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str());
|
||||
}
|
||||
|
||||
Range::~Range()
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Destroying Range", source_name_.c_str());
|
||||
data_sub_.reset();
|
||||
}
|
||||
|
||||
void Range::configure()
|
||||
{
|
||||
Source::configure();
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
std::string source_topic;
|
||||
|
||||
getParameters(source_topic);
|
||||
|
||||
rclcpp::QoS range_qos = rclcpp::SensorDataQoS(); // set to default
|
||||
data_sub_ = node->create_subscription<sensor_msgs::msg::Range>(
|
||||
source_topic, range_qos,
|
||||
std::bind(&Range::dataCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void Range::getData(
|
||||
const rclcpp::Time & curr_time,
|
||||
std::vector<Point> & data) const
|
||||
{
|
||||
// Ignore data from the source if it is not being published yet or
|
||||
// not being published for a long time
|
||||
if (data_ == nullptr) {
|
||||
return;
|
||||
}
|
||||
if (!sourceValid(data_->header.stamp, curr_time)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Ignore data, if its range is out of scope of range sensor abilities
|
||||
if (data_->range < data_->min_range || data_->range > data_->max_range) {
|
||||
RCLCPP_DEBUG(
|
||||
logger_,
|
||||
"[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...",
|
||||
source_name_.c_str(), data_->range, data_->min_range, data_->max_range);
|
||||
return;
|
||||
}
|
||||
|
||||
tf2::Transform tf_transform;
|
||||
if (base_shift_correction_) {
|
||||
// Obtaining the transform to get data from source frame and time where it was received
|
||||
// to the base frame and current time
|
||||
if (
|
||||
!nav2_util::getTransform(
|
||||
data_->header.frame_id, data_->header.stamp,
|
||||
base_frame_id_, curr_time, global_frame_id_,
|
||||
transform_tolerance_, tf_buffer_, tf_transform))
|
||||
{
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// Obtaining the transform to get data from source frame to base frame without time shift
|
||||
// considered. Less accurate but much more faster option not dependent on state estimation
|
||||
// frames.
|
||||
if (
|
||||
!nav2_util::getTransform(
|
||||
data_->header.frame_id, base_frame_id_,
|
||||
transform_tolerance_, tf_buffer_, tf_transform))
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate poses and refill data array
|
||||
float angle;
|
||||
for (
|
||||
angle = -data_->field_of_view / 2;
|
||||
angle < data_->field_of_view / 2;
|
||||
angle += obstacles_angle_)
|
||||
{
|
||||
// Transform point coordinates from source frame -> to base frame
|
||||
tf2::Vector3 p_v3_s(
|
||||
data_->range * std::cos(angle),
|
||||
data_->range * std::sin(angle),
|
||||
0.0);
|
||||
tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
|
||||
|
||||
// Refill data array
|
||||
data.push_back({p_v3_b.x(), p_v3_b.y()});
|
||||
}
|
||||
|
||||
// Make sure that last (field_of_view / 2) point will be in the data array
|
||||
angle = data_->field_of_view / 2;
|
||||
|
||||
// Transform point coordinates from source frame -> to base frame
|
||||
tf2::Vector3 p_v3_s(
|
||||
data_->range * std::cos(angle),
|
||||
data_->range * std::sin(angle),
|
||||
0.0);
|
||||
tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
|
||||
|
||||
// Refill data array
|
||||
data.push_back({p_v3_b.x(), p_v3_b.y()});
|
||||
}
|
||||
|
||||
void Range::getParameters(std::string & source_topic)
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
getCommonParameters(source_topic);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, source_name_ + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 180));
|
||||
obstacles_angle_ = node->get_parameter(source_name_ + ".obstacles_angle").as_double();
|
||||
}
|
||||
|
||||
void Range::dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg)
|
||||
{
|
||||
data_ = msg;
|
||||
}
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
@@ -0,0 +1,126 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "nav2_collision_monitor/scan.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
Scan::Scan(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction)
|
||||
: Source(
|
||||
node, source_name, tf_buffer, base_frame_id, global_frame_id,
|
||||
transform_tolerance, source_timeout, base_shift_correction),
|
||||
data_(nullptr)
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Creating Scan", source_name_.c_str());
|
||||
}
|
||||
|
||||
Scan::~Scan()
|
||||
{
|
||||
RCLCPP_INFO(logger_, "[%s]: Destroying Scan", source_name_.c_str());
|
||||
data_sub_.reset();
|
||||
}
|
||||
|
||||
void Scan::configure()
|
||||
{
|
||||
Source::configure();
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
std::string source_topic;
|
||||
|
||||
// Laser scanner has no own parameters
|
||||
getCommonParameters(source_topic);
|
||||
|
||||
rclcpp::QoS scan_qos = rclcpp::SensorDataQoS(); // set to default
|
||||
data_sub_ = node->create_subscription<sensor_msgs::msg::LaserScan>(
|
||||
source_topic, scan_qos,
|
||||
std::bind(&Scan::dataCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void Scan::getData(
|
||||
const rclcpp::Time & curr_time,
|
||||
std::vector<Point> & data) const
|
||||
{
|
||||
// Ignore data from the source if it is not being published yet or
|
||||
// not being published for a long time
|
||||
if (data_ == nullptr) {
|
||||
return;
|
||||
}
|
||||
if (!sourceValid(data_->header.stamp, curr_time)) {
|
||||
return;
|
||||
}
|
||||
|
||||
tf2::Transform tf_transform;
|
||||
if (base_shift_correction_) {
|
||||
// Obtaining the transform to get data from source frame and time where it was received
|
||||
// to the base frame and current time
|
||||
if (
|
||||
!nav2_util::getTransform(
|
||||
data_->header.frame_id, data_->header.stamp,
|
||||
base_frame_id_, curr_time, global_frame_id_,
|
||||
transform_tolerance_, tf_buffer_, tf_transform))
|
||||
{
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
// Obtaining the transform to get data from source frame to base frame without time shift
|
||||
// considered. Less accurate but much more faster option not dependent on state estimation
|
||||
// frames.
|
||||
if (
|
||||
!nav2_util::getTransform(
|
||||
data_->header.frame_id, base_frame_id_,
|
||||
transform_tolerance_, tf_buffer_, tf_transform))
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate poses and refill data array
|
||||
float angle = data_->angle_min;
|
||||
for (size_t i = 0; i < data_->ranges.size(); i++) {
|
||||
if (data_->ranges[i] >= data_->range_min && data_->ranges[i] <= data_->range_max) {
|
||||
// Transform point coordinates from source frame -> to base frame
|
||||
tf2::Vector3 p_v3_s(
|
||||
data_->ranges[i] * std::cos(angle),
|
||||
data_->ranges[i] * std::sin(angle),
|
||||
0.0);
|
||||
tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
|
||||
|
||||
// Refill data array
|
||||
data.push_back({p_v3_b.x(), p_v3_b.y()});
|
||||
}
|
||||
angle += data_->angle_increment;
|
||||
}
|
||||
}
|
||||
|
||||
void Scan::dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg)
|
||||
{
|
||||
data_ = msg;
|
||||
}
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
@@ -0,0 +1,121 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "nav2_collision_monitor/source.hpp"
|
||||
|
||||
#include <exception>
|
||||
|
||||
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||
|
||||
#include "tf2/convert.h"
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace nav2_collision_monitor
|
||||
{
|
||||
|
||||
Source::Source(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & source_timeout,
|
||||
const bool base_shift_correction)
|
||||
: node_(node), source_name_(source_name), tf_buffer_(tf_buffer),
|
||||
base_frame_id_(base_frame_id), global_frame_id_(global_frame_id),
|
||||
transform_tolerance_(transform_tolerance), source_timeout_(source_timeout),
|
||||
base_shift_correction_(base_shift_correction)
|
||||
{
|
||||
}
|
||||
|
||||
Source::~Source()
|
||||
{
|
||||
}
|
||||
|
||||
bool Source::configure()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
|
||||
// Add callback for dynamic parameters
|
||||
dyn_params_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(&Source::dynamicParametersCallback, this, std::placeholders::_1));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Source::getCommonParameters(std::string & source_topic)
|
||||
{
|
||||
auto node = node_.lock();
|
||||
if (!node) {
|
||||
throw std::runtime_error{"Failed to lock node"};
|
||||
}
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, source_name_ + ".topic",
|
||||
rclcpp::ParameterValue("scan")); // Set deafult topic for laser scanner
|
||||
source_topic = node->get_parameter(source_name_ + ".topic").as_string();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, source_name_ + ".enabled", rclcpp::ParameterValue(true));
|
||||
enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool();
|
||||
}
|
||||
|
||||
bool Source::sourceValid(
|
||||
const rclcpp::Time & source_time,
|
||||
const rclcpp::Time & curr_time) const
|
||||
{
|
||||
// Source is considered as not valid, if latest received data timestamp is earlier
|
||||
// than current time by source_timeout_ interval
|
||||
const rclcpp::Duration dt = curr_time - source_time;
|
||||
if (dt > source_timeout_) {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"[%s]: Latest source and current collision monitor node timestamps differ on %f seconds. "
|
||||
"Ignoring the source.",
|
||||
source_name_.c_str(), dt.seconds());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Source::getEnabled() const
|
||||
{
|
||||
return enabled_;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
Source::dynamicParametersCallback(
|
||||
std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
|
||||
for (auto parameter : parameters) {
|
||||
const auto & param_type = parameter.get_type();
|
||||
const auto & param_name = parameter.get_name();
|
||||
|
||||
if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
|
||||
if (param_name == source_name_ + "." + "enabled") {
|
||||
enabled_ = parameter.as_bool();
|
||||
}
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_collision_monitor
|
||||
@@ -0,0 +1,35 @@
|
||||
# Kinematics test
|
||||
ament_add_gtest(kinematics_test kinematics_test.cpp)
|
||||
ament_target_dependencies(kinematics_test
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(kinematics_test
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Data sources test
|
||||
ament_add_gtest(sources_test sources_test.cpp)
|
||||
ament_target_dependencies(sources_test
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(sources_test
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Polygon shapes test
|
||||
ament_add_gtest(polygons_test polygons_test.cpp)
|
||||
ament_target_dependencies(polygons_test
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(polygons_test
|
||||
${library_name}
|
||||
)
|
||||
|
||||
# Collision Monitor node test
|
||||
ament_add_gtest(collision_monitor_node_test collision_monitor_node_test.cpp)
|
||||
ament_target_dependencies(collision_monitor_node_test
|
||||
${dependencies}
|
||||
)
|
||||
target_link_libraries(collision_monitor_node_test
|
||||
${library_name}
|
||||
)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,98 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
#include <chrono>
|
||||
#include <vector>
|
||||
#include <limits>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "nav2_collision_monitor/types.hpp"
|
||||
#include "nav2_collision_monitor/kinematics.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
static constexpr double EPSILON = std::numeric_limits<float>::epsilon();
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
TEST(KinematicsTest, testTransformPoints)
|
||||
{
|
||||
// Transform: move frame to (2.0, 1.0) coordinate and rotate it on 30 degrees
|
||||
const nav2_collision_monitor::Pose tf{2.0, 1.0, M_PI / 6.0};
|
||||
// Add two points in the basic frame
|
||||
std::vector<nav2_collision_monitor::Point> points;
|
||||
points.push_back({3.0, 2.0});
|
||||
points.push_back({0.0, 0.0});
|
||||
|
||||
// Transform points from basic frame to the new frame
|
||||
nav2_collision_monitor::transformPoints(tf, points);
|
||||
|
||||
// Check that all points were transformed correctly
|
||||
// Distance to point in a new frame
|
||||
double new_point_distance = std::sqrt(1.0 + 1.0);
|
||||
// Angle of point in a new frame. Calculated as:
|
||||
// angle of point in a moved frame - frame rotation.
|
||||
double new_point_angle = M_PI / 4.0 - M_PI / 6.0;
|
||||
EXPECT_NEAR(points[0].x, new_point_distance * std::cos(new_point_angle), EPSILON);
|
||||
EXPECT_NEAR(points[0].y, new_point_distance * std::sin(new_point_angle), EPSILON);
|
||||
|
||||
new_point_distance = std::sqrt(1.0 + 4.0);
|
||||
new_point_angle = M_PI + std::atan(1.0 / 2.0) - M_PI / 6.0;
|
||||
EXPECT_NEAR(points[1].x, new_point_distance * std::cos(new_point_angle), EPSILON);
|
||||
EXPECT_NEAR(points[1].y, new_point_distance * std::sin(new_point_angle), EPSILON);
|
||||
}
|
||||
|
||||
TEST(KinematicsTest, testProjectState)
|
||||
{
|
||||
// Y Y
|
||||
// ^ ^
|
||||
// ' '
|
||||
// ' ==> ' *
|
||||
// ' * <- robot's nose 2.0' o <- moved robot
|
||||
// 1.0' o <- robot's back '
|
||||
// ..........>X ..........>X
|
||||
// 2.0 2.0
|
||||
|
||||
// Initial pose of robot
|
||||
nav2_collision_monitor::Pose pose{2.0, 1.0, M_PI / 4.0};
|
||||
// Initial velocity of robot
|
||||
nav2_collision_monitor::Velocity vel{0.0, 1.0, M_PI / 4.0};
|
||||
const double dt = 1.0;
|
||||
|
||||
// Moving robot and rotating velocity
|
||||
nav2_collision_monitor::projectState(dt, pose, vel);
|
||||
|
||||
// Check pose of moved and rotated robot
|
||||
EXPECT_NEAR(pose.x, 2.0, EPSILON);
|
||||
EXPECT_NEAR(pose.y, 2.0, EPSILON);
|
||||
EXPECT_NEAR(pose.theta, M_PI / 2, EPSILON);
|
||||
|
||||
// Check rotated velocity
|
||||
// Rotated velocity angle is an initial velocity angle + rotation
|
||||
const double rotated_vel_angle = M_PI / 2.0 + M_PI / 4.0;
|
||||
EXPECT_NEAR(vel.x, std::cos(rotated_vel_angle), EPSILON);
|
||||
EXPECT_NEAR(vel.y, std::sin(rotated_vel_angle), EPSILON);
|
||||
EXPECT_NEAR(vel.tw, M_PI / 4.0, EPSILON); // should be the same
|
||||
}
|
||||
@@ -0,0 +1,698 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "geometry_msgs/msg/point32.hpp"
|
||||
#include "geometry_msgs/msg/polygon_stamped.hpp"
|
||||
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
|
||||
#include "nav2_collision_monitor/types.hpp"
|
||||
#include "nav2_collision_monitor/polygon.hpp"
|
||||
#include "nav2_collision_monitor/circle.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
static constexpr double EPSILON = std::numeric_limits<float>::epsilon();
|
||||
|
||||
static const char BASE_FRAME_ID[]{"base_link"};
|
||||
static const char FOOTPRINT_TOPIC[]{"footprint"};
|
||||
static const char POLYGON_PUB_TOPIC[]{"polygon"};
|
||||
static const char POLYGON_NAME[]{"TestPolygon"};
|
||||
static const char CIRCLE_NAME[]{"TestCircle"};
|
||||
static const std::vector<double> SQUARE_POLYGON {
|
||||
0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5};
|
||||
static const std::vector<double> ARBITRARY_POLYGON {
|
||||
1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 2.0, -1.0, -1.0, -1.0, -1.0, 1.0};
|
||||
static const double CIRCLE_RADIUS{0.5};
|
||||
static const int MAX_POINTS{1};
|
||||
static const double SLOWDOWN_RATIO{0.7};
|
||||
static const double TIME_BEFORE_COLLISION{1.0};
|
||||
static const double SIMULATION_TIME_STEP{0.01};
|
||||
static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)};
|
||||
|
||||
class TestNode : public nav2_util::LifecycleNode
|
||||
{
|
||||
public:
|
||||
TestNode()
|
||||
: nav2_util::LifecycleNode("test_node"), polygon_received_(nullptr)
|
||||
{
|
||||
polygon_sub_ = this->create_subscription<geometry_msgs::msg::PolygonStamped>(
|
||||
POLYGON_PUB_TOPIC, rclcpp::SystemDefaultsQoS(),
|
||||
std::bind(&TestNode::polygonCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
~TestNode()
|
||||
{
|
||||
footprint_pub_.reset();
|
||||
}
|
||||
|
||||
void publishFootprint()
|
||||
{
|
||||
footprint_pub_ = this->create_publisher<geometry_msgs::msg::PolygonStamped>(
|
||||
FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
|
||||
|
||||
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 = this->now();
|
||||
|
||||
geometry_msgs::msg::Point32 p;
|
||||
for (unsigned int i = 0; i < SQUARE_POLYGON.size(); i = i + 2) {
|
||||
p.x = SQUARE_POLYGON[i];
|
||||
p.y = SQUARE_POLYGON[i + 1];
|
||||
msg->polygon.points.push_back(p);
|
||||
}
|
||||
|
||||
footprint_pub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
void polygonCallback(geometry_msgs::msg::PolygonStamped::SharedPtr msg)
|
||||
{
|
||||
polygon_received_ = msg;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PolygonStamped::SharedPtr waitPolygonReceived(
|
||||
const std::chrono::nanoseconds & timeout)
|
||||
{
|
||||
rclcpp::Time start_time = this->now();
|
||||
while (rclcpp::ok() && this->now() - start_time <= rclcpp::Duration(timeout)) {
|
||||
if (polygon_received_) {
|
||||
return polygon_received_;
|
||||
}
|
||||
rclcpp::spin_some(this->get_node_base_interface());
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;
|
||||
|
||||
geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received_;
|
||||
}; // TestNode
|
||||
|
||||
class PolygonWrapper : public nav2_collision_monitor::Polygon
|
||||
{
|
||||
public:
|
||||
PolygonWrapper(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & polygon_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const tf2::Duration & transform_tolerance)
|
||||
: nav2_collision_monitor::Polygon(
|
||||
node, polygon_name, tf_buffer, base_frame_id, transform_tolerance)
|
||||
{
|
||||
}
|
||||
|
||||
double getSimulationTimeStep() const
|
||||
{
|
||||
return simulation_time_step_;
|
||||
}
|
||||
|
||||
double isVisualize() const
|
||||
{
|
||||
return visualize_;
|
||||
}
|
||||
}; // PolygonWrapper
|
||||
|
||||
class CircleWrapper : public nav2_collision_monitor::Circle
|
||||
{
|
||||
public:
|
||||
CircleWrapper(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & polygon_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const tf2::Duration & transform_tolerance)
|
||||
: nav2_collision_monitor::Circle(
|
||||
node, polygon_name, tf_buffer, base_frame_id, transform_tolerance)
|
||||
{
|
||||
}
|
||||
|
||||
double getRadius() const
|
||||
{
|
||||
return radius_;
|
||||
}
|
||||
|
||||
double getRadiusSquared() const
|
||||
{
|
||||
return radius_squared_;
|
||||
}
|
||||
}; // CircleWrapper
|
||||
|
||||
class Tester : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
Tester();
|
||||
~Tester();
|
||||
|
||||
protected:
|
||||
// Working with parameters
|
||||
void setCommonParameters(const std::string & polygon_name, const std::string & action_type);
|
||||
void setPolygonParameters(const std::vector<double> & points);
|
||||
void setCircleParameters(const double radius);
|
||||
bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param);
|
||||
// Creating routines
|
||||
void createPolygon(const std::string & action_type);
|
||||
void createCircle(const std::string & action_type);
|
||||
|
||||
// Wait until footprint will be received
|
||||
bool waitFootprint(
|
||||
const std::chrono::nanoseconds & timeout,
|
||||
std::vector<nav2_collision_monitor::Point> & footprint);
|
||||
|
||||
std::shared_ptr<TestNode> test_node_;
|
||||
|
||||
std::shared_ptr<PolygonWrapper> polygon_;
|
||||
std::shared_ptr<CircleWrapper> circle_;
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
}; // Tester
|
||||
|
||||
Tester::Tester()
|
||||
{
|
||||
test_node_ = std::make_shared<TestNode>();
|
||||
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(test_node_->get_clock());
|
||||
tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
}
|
||||
|
||||
Tester::~Tester()
|
||||
{
|
||||
polygon_.reset();
|
||||
circle_.reset();
|
||||
|
||||
test_node_.reset();
|
||||
|
||||
tf_listener_.reset();
|
||||
tf_buffer_.reset();
|
||||
}
|
||||
|
||||
void Tester::setCommonParameters(const std::string & polygon_name, const std::string & action_type)
|
||||
{
|
||||
test_node_->declare_parameter(
|
||||
polygon_name + ".action_type", rclcpp::ParameterValue(action_type));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(polygon_name + ".action_type", action_type));
|
||||
|
||||
test_node_->declare_parameter(
|
||||
polygon_name + ".max_points", rclcpp::ParameterValue(MAX_POINTS));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(polygon_name + ".max_points", MAX_POINTS));
|
||||
|
||||
test_node_->declare_parameter(
|
||||
polygon_name + ".slowdown_ratio", rclcpp::ParameterValue(SLOWDOWN_RATIO));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO));
|
||||
|
||||
test_node_->declare_parameter(
|
||||
polygon_name + ".time_before_collision",
|
||||
rclcpp::ParameterValue(TIME_BEFORE_COLLISION));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(polygon_name + ".time_before_collision", TIME_BEFORE_COLLISION));
|
||||
|
||||
test_node_->declare_parameter(
|
||||
polygon_name + ".simulation_time_step", rclcpp::ParameterValue(SIMULATION_TIME_STEP));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(polygon_name + ".simulation_time_step", SIMULATION_TIME_STEP));
|
||||
|
||||
test_node_->declare_parameter(
|
||||
polygon_name + ".visualize", rclcpp::ParameterValue(true));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(polygon_name + ".visualize", true));
|
||||
|
||||
test_node_->declare_parameter(
|
||||
polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(POLYGON_PUB_TOPIC));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC));
|
||||
}
|
||||
|
||||
void Tester::setPolygonParameters(const std::vector<double> & points)
|
||||
{
|
||||
test_node_->declare_parameter(
|
||||
std::string(POLYGON_NAME) + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(POLYGON_NAME) + ".footprint_topic", FOOTPRINT_TOPIC));
|
||||
|
||||
test_node_->declare_parameter(
|
||||
std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points));
|
||||
}
|
||||
|
||||
void Tester::setCircleParameters(const double radius)
|
||||
{
|
||||
test_node_->declare_parameter(
|
||||
std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius));
|
||||
}
|
||||
|
||||
bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const std::string & param)
|
||||
{
|
||||
bool ret = false;
|
||||
|
||||
// Check that parameter is not set after configuring
|
||||
try {
|
||||
test_node_->get_parameter(polygon_name + "." + param);
|
||||
} catch (std::exception & ex) {
|
||||
std::string message = ex.what();
|
||||
if (message.find("." + param) != std::string::npos &&
|
||||
message.find("is not initialized") != std::string::npos)
|
||||
{
|
||||
ret = true;
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void Tester::createPolygon(const std::string & action_type)
|
||||
{
|
||||
setCommonParameters(POLYGON_NAME, action_type);
|
||||
setPolygonParameters(SQUARE_POLYGON);
|
||||
|
||||
polygon_ = std::make_shared<PolygonWrapper>(
|
||||
test_node_, POLYGON_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_TRUE(polygon_->configure());
|
||||
polygon_->activate();
|
||||
}
|
||||
|
||||
void Tester::createCircle(const std::string & action_type)
|
||||
{
|
||||
setCommonParameters(CIRCLE_NAME, action_type);
|
||||
setCircleParameters(CIRCLE_RADIUS);
|
||||
|
||||
circle_ = std::make_shared<CircleWrapper>(
|
||||
test_node_, CIRCLE_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_TRUE(circle_->configure());
|
||||
circle_->activate();
|
||||
}
|
||||
|
||||
bool Tester::waitFootprint(
|
||||
const std::chrono::nanoseconds & timeout,
|
||||
std::vector<nav2_collision_monitor::Point> & footprint)
|
||||
{
|
||||
rclcpp::Time start_time = test_node_->now();
|
||||
while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) {
|
||||
polygon_->updatePolygon();
|
||||
polygon_->getPolygon(footprint);
|
||||
if (footprint.size() > 0) {
|
||||
return true;
|
||||
}
|
||||
rclcpp::spin_some(test_node_->get_node_base_interface());
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonGetStopParameters)
|
||||
{
|
||||
createPolygon("stop");
|
||||
|
||||
// Check that common parameters set correctly
|
||||
EXPECT_EQ(polygon_->getName(), POLYGON_NAME);
|
||||
EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::STOP);
|
||||
EXPECT_EQ(polygon_->getMaxPoints(), MAX_POINTS);
|
||||
EXPECT_EQ(polygon_->isVisualize(), true);
|
||||
|
||||
// Check that polygon set correctly
|
||||
std::vector<nav2_collision_monitor::Point> poly;
|
||||
polygon_->getPolygon(poly);
|
||||
ASSERT_EQ(poly.size(), 4u);
|
||||
EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0], EPSILON);
|
||||
EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1], EPSILON);
|
||||
EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2], EPSILON);
|
||||
EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3], EPSILON);
|
||||
EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4], EPSILON);
|
||||
EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5], EPSILON);
|
||||
EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6], EPSILON);
|
||||
EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonGetSlowdownParameters)
|
||||
{
|
||||
createPolygon("slowdown");
|
||||
|
||||
// Check that common parameters set correctly
|
||||
EXPECT_EQ(polygon_->getName(), POLYGON_NAME);
|
||||
EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::SLOWDOWN);
|
||||
EXPECT_EQ(polygon_->getMaxPoints(), MAX_POINTS);
|
||||
EXPECT_EQ(polygon_->isVisualize(), true);
|
||||
// Check that slowdown_ratio is correct
|
||||
EXPECT_NEAR(polygon_->getSlowdownRatio(), SLOWDOWN_RATIO, EPSILON);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonGetApproachParameters)
|
||||
{
|
||||
createPolygon("approach");
|
||||
|
||||
// Check that common parameters set correctly
|
||||
EXPECT_EQ(polygon_->getName(), POLYGON_NAME);
|
||||
EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::APPROACH);
|
||||
EXPECT_EQ(polygon_->getMaxPoints(), MAX_POINTS);
|
||||
EXPECT_EQ(polygon_->isVisualize(), true);
|
||||
// Check that time_before_collision and simulation_time_step are correct
|
||||
EXPECT_NEAR(polygon_->getTimeBeforeCollision(), TIME_BEFORE_COLLISION, EPSILON);
|
||||
EXPECT_NEAR(polygon_->getSimulationTimeStep(), SIMULATION_TIME_STEP, EPSILON);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testCircleGetParameters)
|
||||
{
|
||||
createCircle("approach");
|
||||
|
||||
// Check that common parameters set correctly
|
||||
EXPECT_EQ(circle_->getName(), CIRCLE_NAME);
|
||||
EXPECT_EQ(circle_->getActionType(), nav2_collision_monitor::APPROACH);
|
||||
EXPECT_EQ(circle_->getMaxPoints(), MAX_POINTS);
|
||||
|
||||
// Check that Circle-specific parameters were set correctly
|
||||
EXPECT_NEAR(circle_->getRadius(), CIRCLE_RADIUS, EPSILON);
|
||||
EXPECT_NEAR(circle_->getRadiusSquared(), CIRCLE_RADIUS * CIRCLE_RADIUS, EPSILON);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonUndeclaredActionType)
|
||||
{
|
||||
// "action_type" parameter is not initialized
|
||||
polygon_ = std::make_shared<PolygonWrapper>(
|
||||
test_node_, POLYGON_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_FALSE(polygon_->configure());
|
||||
// Check that "action_type" parameter is not set after configuring
|
||||
ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "action_type"));
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonUndeclaredPoints)
|
||||
{
|
||||
// "points" parameter is not initialized
|
||||
test_node_->declare_parameter(
|
||||
std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop"));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop"));
|
||||
polygon_ = std::make_shared<PolygonWrapper>(
|
||||
test_node_, POLYGON_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_FALSE(polygon_->configure());
|
||||
// Check that "points" parameter is not set after configuring
|
||||
ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "points"));
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonIncorrectActionType)
|
||||
{
|
||||
setCommonParameters(POLYGON_NAME, "incorrect_action_type");
|
||||
setPolygonParameters(SQUARE_POLYGON);
|
||||
|
||||
polygon_ = std::make_shared<PolygonWrapper>(
|
||||
test_node_, POLYGON_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_FALSE(polygon_->configure());
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonIncorrectPoints1)
|
||||
{
|
||||
setCommonParameters(POLYGON_NAME, "stop");
|
||||
|
||||
std::vector<double> incorrect_points = SQUARE_POLYGON;
|
||||
incorrect_points.resize(6); // Not enough for triangle
|
||||
test_node_->declare_parameter(
|
||||
std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points));
|
||||
|
||||
polygon_ = std::make_shared<PolygonWrapper>(
|
||||
test_node_, POLYGON_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_FALSE(polygon_->configure());
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonIncorrectPoints2)
|
||||
{
|
||||
setCommonParameters(POLYGON_NAME, "stop");
|
||||
|
||||
std::vector<double> incorrect_points = SQUARE_POLYGON;
|
||||
incorrect_points.resize(9); // Odd number of points
|
||||
test_node_->declare_parameter(
|
||||
std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(incorrect_points));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", incorrect_points));
|
||||
|
||||
polygon_ = std::make_shared<PolygonWrapper>(
|
||||
test_node_, POLYGON_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_FALSE(polygon_->configure());
|
||||
}
|
||||
|
||||
TEST_F(Tester, testCircleUndeclaredRadius)
|
||||
{
|
||||
setCommonParameters(CIRCLE_NAME, "stop");
|
||||
|
||||
circle_ = std::make_shared<CircleWrapper>(
|
||||
test_node_, CIRCLE_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_FALSE(circle_->configure());
|
||||
|
||||
// Check that "radius" parameter is not set after configuring
|
||||
ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius"));
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonUpdate)
|
||||
{
|
||||
createPolygon("approach");
|
||||
|
||||
std::vector<nav2_collision_monitor::Point> poly;
|
||||
polygon_->getPolygon(poly);
|
||||
ASSERT_EQ(poly.size(), 0u);
|
||||
|
||||
test_node_->publishFootprint();
|
||||
|
||||
std::vector<nav2_collision_monitor::Point> footprint;
|
||||
ASSERT_TRUE(waitFootprint(500ms, footprint));
|
||||
|
||||
ASSERT_EQ(footprint.size(), 4u);
|
||||
EXPECT_NEAR(footprint[0].x, SQUARE_POLYGON[0], EPSILON);
|
||||
EXPECT_NEAR(footprint[0].y, SQUARE_POLYGON[1], EPSILON);
|
||||
EXPECT_NEAR(footprint[1].x, SQUARE_POLYGON[2], EPSILON);
|
||||
EXPECT_NEAR(footprint[1].y, SQUARE_POLYGON[3], EPSILON);
|
||||
EXPECT_NEAR(footprint[2].x, SQUARE_POLYGON[4], EPSILON);
|
||||
EXPECT_NEAR(footprint[2].y, SQUARE_POLYGON[5], EPSILON);
|
||||
EXPECT_NEAR(footprint[3].x, SQUARE_POLYGON[6], EPSILON);
|
||||
EXPECT_NEAR(footprint[3].y, SQUARE_POLYGON[7], EPSILON);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonGetPointsInside)
|
||||
{
|
||||
createPolygon("stop");
|
||||
|
||||
std::vector<nav2_collision_monitor::Point> points;
|
||||
|
||||
// Out of boundaries points
|
||||
points.push_back({1.0, 0.0});
|
||||
points.push_back({0.0, 1.0});
|
||||
points.push_back({-1.0, 0.0});
|
||||
points.push_back({0.0, -1.0});
|
||||
ASSERT_EQ(polygon_->getPointsInside(points), 0);
|
||||
|
||||
// Add one point inside
|
||||
points.push_back({-0.1, 0.3});
|
||||
ASSERT_EQ(polygon_->getPointsInside(points), 1);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonGetPointsInsideEdge)
|
||||
{
|
||||
// Test for checking edge cases in raytracing algorithm.
|
||||
// All points are lie on the edge lines parallel to OX, where the raytracing takes place.
|
||||
setCommonParameters(POLYGON_NAME, "stop");
|
||||
setPolygonParameters(ARBITRARY_POLYGON);
|
||||
|
||||
polygon_ = std::make_shared<PolygonWrapper>(
|
||||
test_node_, POLYGON_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_TRUE(polygon_->configure());
|
||||
|
||||
std::vector<nav2_collision_monitor::Point> points;
|
||||
|
||||
// Out of boundaries points
|
||||
points.push_back({-2.0, -1.0});
|
||||
points.push_back({-2.0, 0.0});
|
||||
points.push_back({-2.0, 1.0});
|
||||
points.push_back({3.0, -1.0});
|
||||
points.push_back({3.0, 0.0});
|
||||
points.push_back({3.0, 1.0});
|
||||
ASSERT_EQ(polygon_->getPointsInside(points), 0);
|
||||
|
||||
// Add one point inside
|
||||
points.push_back({0.0, 0.0});
|
||||
ASSERT_EQ(polygon_->getPointsInside(points), 1);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testCircleGetPointsInside)
|
||||
{
|
||||
createCircle("stop");
|
||||
|
||||
std::vector<nav2_collision_monitor::Point> points;
|
||||
// Point out of radius
|
||||
points.push_back({1.0, 0.0});
|
||||
ASSERT_EQ(circle_->getPointsInside(points), 0);
|
||||
|
||||
// Add one point inside
|
||||
points.push_back({-0.1, 0.3});
|
||||
ASSERT_EQ(circle_->getPointsInside(points), 1);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonGetCollisionTime)
|
||||
{
|
||||
createPolygon("approach");
|
||||
|
||||
// Set footprint for Polygon
|
||||
test_node_->publishFootprint();
|
||||
std::vector<nav2_collision_monitor::Point> footprint;
|
||||
ASSERT_TRUE(waitFootprint(500ms, footprint));
|
||||
ASSERT_EQ(footprint.size(), 4u);
|
||||
|
||||
// Forward movement check
|
||||
nav2_collision_monitor::Velocity vel{0.5, 0.0, 0.0}; // 0.5 m/s forward movement
|
||||
// Two points 0.2 m ahead the footprint (0.5 m)
|
||||
std::vector<nav2_collision_monitor::Point> points{{0.7, -0.01}, {0.7, 0.01}};
|
||||
// Collision is expected to be ~= 0.2 m / 0.5 m/s seconds
|
||||
EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP);
|
||||
|
||||
// Backward movement check
|
||||
vel = {-0.5, 0.0, 0.0}; // 0.5 m/s backward movement
|
||||
// Two points 0.2 m behind the footprint (0.5 m)
|
||||
points.clear();
|
||||
points = {{-0.7, -0.01}, {-0.7, 0.01}};
|
||||
// Collision is expected to be in ~= 0.2 m / 0.5 m/s seconds
|
||||
EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP);
|
||||
|
||||
// Sideway movement check
|
||||
vel = {0.0, 0.5, 0.0}; // 0.5 m/s sideway movement
|
||||
// Two points 0.1 m ahead the footprint (0.5 m)
|
||||
points.clear();
|
||||
points = {{-0.01, 0.6}, {0.01, 0.6}};
|
||||
// Collision is expected to be in ~= 0.1 m / 0.5 m/s seconds
|
||||
EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.2, SIMULATION_TIME_STEP);
|
||||
|
||||
// Rotation check
|
||||
vel = {0.0, 0.0, 1.0}; // 1.0 rad/s rotation
|
||||
// ^ OX
|
||||
// '
|
||||
// x'x <- 2 collision points
|
||||
// '
|
||||
// ----------- <- robot footprint
|
||||
// OY | ' |
|
||||
// <...|....o....|...
|
||||
// | ' |
|
||||
// -----------
|
||||
// '
|
||||
points.clear();
|
||||
points = {{0.49, -0.01}, {0.49, 0.01}};
|
||||
// Collision is expected to be in ~= 45 degrees * M_PI / (180 degrees * 1.0 rad/s) seconds
|
||||
double exp_res = 45 / 180 * M_PI;
|
||||
EXPECT_NEAR(polygon_->getCollisionTime(points, vel), exp_res, EPSILON);
|
||||
|
||||
// Two points are already inside footprint
|
||||
vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement
|
||||
// Two points inside
|
||||
points.clear();
|
||||
points = {{0.1, -0.01}, {0.1, 0.01}};
|
||||
// Collision already appeared: collision time should be 0
|
||||
EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.0, EPSILON);
|
||||
|
||||
// All points are out of simulation prediction
|
||||
vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement
|
||||
// Two points 0.6 m ahead the footprint (0.5 m)
|
||||
points.clear();
|
||||
points = {{1.1, -0.01}, {1.1, 0.01}};
|
||||
// There is no collision: return value should be negative
|
||||
EXPECT_LT(polygon_->getCollisionTime(points, vel), 0.0);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonPublish)
|
||||
{
|
||||
createPolygon("stop");
|
||||
polygon_->publish();
|
||||
geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received =
|
||||
test_node_->waitPolygonReceived(500ms);
|
||||
|
||||
ASSERT_NE(polygon_received, nullptr);
|
||||
ASSERT_EQ(polygon_received->polygon.points.size(), 4u);
|
||||
EXPECT_NEAR(polygon_received->polygon.points[0].x, SQUARE_POLYGON[0], EPSILON);
|
||||
EXPECT_NEAR(polygon_received->polygon.points[0].y, SQUARE_POLYGON[1], EPSILON);
|
||||
EXPECT_NEAR(polygon_received->polygon.points[1].x, SQUARE_POLYGON[2], EPSILON);
|
||||
EXPECT_NEAR(polygon_received->polygon.points[1].y, SQUARE_POLYGON[3], EPSILON);
|
||||
EXPECT_NEAR(polygon_received->polygon.points[2].x, SQUARE_POLYGON[4], EPSILON);
|
||||
EXPECT_NEAR(polygon_received->polygon.points[2].y, SQUARE_POLYGON[5], EPSILON);
|
||||
EXPECT_NEAR(polygon_received->polygon.points[3].x, SQUARE_POLYGON[6], EPSILON);
|
||||
EXPECT_NEAR(polygon_received->polygon.points[3].y, SQUARE_POLYGON[7], EPSILON);
|
||||
|
||||
polygon_->deactivate();
|
||||
}
|
||||
|
||||
TEST_F(Tester, testPolygonDefaultVisualize)
|
||||
{
|
||||
// Use default parameters, visualize should be false by-default
|
||||
test_node_->declare_parameter(
|
||||
std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop"));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop"));
|
||||
setPolygonParameters(SQUARE_POLYGON);
|
||||
|
||||
// Create new polygon
|
||||
polygon_ = std::make_shared<PolygonWrapper>(
|
||||
test_node_, POLYGON_NAME,
|
||||
tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
|
||||
ASSERT_TRUE(polygon_->configure());
|
||||
polygon_->activate();
|
||||
|
||||
// Try to publish polygon
|
||||
polygon_->publish();
|
||||
|
||||
// Wait for polygon: it should not be published
|
||||
ASSERT_EQ(test_node_->waitPolygonReceived(100ms), nullptr);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
// Initialize the system
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
// Actual testing
|
||||
bool test_result = RUN_ALL_TESTS();
|
||||
|
||||
// Shutdown
|
||||
rclcpp::shutdown();
|
||||
|
||||
return test_result;
|
||||
}
|
||||
@@ -0,0 +1,635 @@
|
||||
// Copyright (c) 2022 Samsung R&D Institute Russia
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <cmath>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
#include "sensor_msgs/msg/range.hpp"
|
||||
#include "sensor_msgs/point_cloud2_iterator.hpp"
|
||||
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
|
||||
#include "nav2_collision_monitor/types.hpp"
|
||||
#include "nav2_collision_monitor/scan.hpp"
|
||||
#include "nav2_collision_monitor/pointcloud.hpp"
|
||||
#include "nav2_collision_monitor/range.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
static constexpr double EPSILON = std::numeric_limits<float>::epsilon();
|
||||
|
||||
static const char BASE_FRAME_ID[]{"base_link"};
|
||||
static const char SOURCE_FRAME_ID[]{"base_source"};
|
||||
static const char GLOBAL_FRAME_ID[]{"odom"};
|
||||
static const char SCAN_NAME[]{"LaserScan"};
|
||||
static const char SCAN_TOPIC[]{"scan"};
|
||||
static const char POINTCLOUD_NAME[]{"PointCloud"};
|
||||
static const char POINTCLOUD_TOPIC[]{"pointcloud"};
|
||||
static const char RANGE_NAME[]{"Range"};
|
||||
static const char RANGE_TOPIC[]{"range"};
|
||||
static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)};
|
||||
static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)};
|
||||
|
||||
class TestNode : public nav2_util::LifecycleNode
|
||||
{
|
||||
public:
|
||||
TestNode()
|
||||
: nav2_util::LifecycleNode("test_node")
|
||||
{
|
||||
}
|
||||
|
||||
~TestNode()
|
||||
{
|
||||
scan_pub_.reset();
|
||||
pointcloud_pub_.reset();
|
||||
range_pub_.reset();
|
||||
}
|
||||
|
||||
void publishScan(const rclcpp::Time & stamp, const double range)
|
||||
{
|
||||
scan_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>(
|
||||
SCAN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
|
||||
|
||||
std::unique_ptr<sensor_msgs::msg::LaserScan> msg =
|
||||
std::make_unique<sensor_msgs::msg::LaserScan>();
|
||||
|
||||
msg->header.frame_id = SOURCE_FRAME_ID;
|
||||
msg->header.stamp = stamp;
|
||||
|
||||
msg->angle_min = 0.0;
|
||||
msg->angle_max = 2 * M_PI;
|
||||
msg->angle_increment = M_PI / 2;
|
||||
msg->time_increment = 0.0;
|
||||
msg->scan_time = 0.0;
|
||||
msg->range_min = 0.1;
|
||||
msg->range_max = 1.1;
|
||||
std::vector<float> ranges(4, range);
|
||||
msg->ranges = ranges;
|
||||
|
||||
scan_pub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
void publishPointCloud(const rclcpp::Time & stamp)
|
||||
{
|
||||
pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
|
||||
POINTCLOUD_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
|
||||
|
||||
std::unique_ptr<sensor_msgs::msg::PointCloud2> msg =
|
||||
std::make_unique<sensor_msgs::msg::PointCloud2>();
|
||||
sensor_msgs::PointCloud2Modifier modifier(*msg);
|
||||
|
||||
msg->header.frame_id = SOURCE_FRAME_ID;
|
||||
msg->header.stamp = stamp;
|
||||
|
||||
modifier.setPointCloud2Fields(
|
||||
3, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
|
||||
"y", 1, sensor_msgs::msg::PointField::FLOAT32,
|
||||
"z", 1, sensor_msgs::msg::PointField::FLOAT32);
|
||||
modifier.resize(3);
|
||||
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");
|
||||
|
||||
// Point 0: (0.5, 0.5, 0.2)
|
||||
*iter_x = 0.5;
|
||||
*iter_y = 0.5;
|
||||
*iter_z = 0.2;
|
||||
++iter_x; ++iter_y; ++iter_z;
|
||||
|
||||
// Point 1: (-0.5, -0.5, 0.3)
|
||||
*iter_x = -0.5;
|
||||
*iter_y = -0.5;
|
||||
*iter_z = 0.3;
|
||||
++iter_x; ++iter_y; ++iter_z;
|
||||
|
||||
// Point 2: (1.0, 1.0, 10.0)
|
||||
*iter_x = 1.0;
|
||||
*iter_y = 1.0;
|
||||
*iter_z = 10.0;
|
||||
|
||||
pointcloud_pub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
void publishRange(const rclcpp::Time & stamp, const double range)
|
||||
{
|
||||
range_pub_ = this->create_publisher<sensor_msgs::msg::Range>(
|
||||
RANGE_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
|
||||
|
||||
std::unique_ptr<sensor_msgs::msg::Range> msg =
|
||||
std::make_unique<sensor_msgs::msg::Range>();
|
||||
|
||||
msg->header.frame_id = SOURCE_FRAME_ID;
|
||||
msg->header.stamp = stamp;
|
||||
|
||||
msg->radiation_type = 0;
|
||||
msg->field_of_view = M_PI / 10;
|
||||
msg->min_range = 0.1;
|
||||
msg->max_range = 1.1;
|
||||
msg->range = range;
|
||||
|
||||
range_pub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Range>::SharedPtr range_pub_;
|
||||
}; // TestNode
|
||||
|
||||
class ScanWrapper : public nav2_collision_monitor::Scan
|
||||
{
|
||||
public:
|
||||
ScanWrapper(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & data_timeout,
|
||||
const bool base_shift_correction)
|
||||
: nav2_collision_monitor::Scan(
|
||||
node, source_name, tf_buffer, base_frame_id, global_frame_id,
|
||||
transform_tolerance, data_timeout, base_shift_correction)
|
||||
{}
|
||||
|
||||
bool dataReceived() const
|
||||
{
|
||||
return data_ != nullptr;
|
||||
}
|
||||
}; // ScanWrapper
|
||||
|
||||
class PointCloudWrapper : public nav2_collision_monitor::PointCloud
|
||||
{
|
||||
public:
|
||||
PointCloudWrapper(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & data_timeout,
|
||||
const bool base_shift_correction)
|
||||
: nav2_collision_monitor::PointCloud(
|
||||
node, source_name, tf_buffer, base_frame_id, global_frame_id,
|
||||
transform_tolerance, data_timeout, base_shift_correction)
|
||||
{}
|
||||
|
||||
bool dataReceived() const
|
||||
{
|
||||
return data_ != nullptr;
|
||||
}
|
||||
}; // PointCloudWrapper
|
||||
|
||||
class RangeWrapper : public nav2_collision_monitor::Range
|
||||
{
|
||||
public:
|
||||
RangeWrapper(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & source_name,
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
||||
const std::string & base_frame_id,
|
||||
const std::string & global_frame_id,
|
||||
const tf2::Duration & transform_tolerance,
|
||||
const rclcpp::Duration & data_timeout,
|
||||
const bool base_shift_correction)
|
||||
: nav2_collision_monitor::Range(
|
||||
node, source_name, tf_buffer, base_frame_id, global_frame_id,
|
||||
transform_tolerance, data_timeout, base_shift_correction)
|
||||
{}
|
||||
|
||||
bool dataReceived() const
|
||||
{
|
||||
return data_ != nullptr;
|
||||
}
|
||||
}; // RangeWrapper
|
||||
|
||||
class Tester : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
Tester();
|
||||
~Tester();
|
||||
|
||||
protected:
|
||||
// Data sources creation routine
|
||||
void createSources(const bool base_shift_correction = true);
|
||||
|
||||
// Setting TF chains
|
||||
void sendTransforms(const rclcpp::Time & stamp);
|
||||
|
||||
// Data sources working routines
|
||||
bool waitScan(const std::chrono::nanoseconds & timeout);
|
||||
bool waitPointCloud(const std::chrono::nanoseconds & timeout);
|
||||
bool waitRange(const std::chrono::nanoseconds & timeout);
|
||||
void checkScan(const std::vector<nav2_collision_monitor::Point> & data);
|
||||
void checkPointCloud(const std::vector<nav2_collision_monitor::Point> & data);
|
||||
void checkRange(const std::vector<nav2_collision_monitor::Point> & data);
|
||||
|
||||
std::shared_ptr<TestNode> test_node_;
|
||||
std::shared_ptr<ScanWrapper> scan_;
|
||||
std::shared_ptr<PointCloudWrapper> pointcloud_;
|
||||
std::shared_ptr<RangeWrapper> range_;
|
||||
|
||||
private:
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
}; // Tester
|
||||
|
||||
Tester::Tester()
|
||||
{
|
||||
test_node_ = std::make_shared<TestNode>();
|
||||
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(test_node_->get_clock());
|
||||
tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
}
|
||||
|
||||
Tester::~Tester()
|
||||
{
|
||||
scan_.reset();
|
||||
pointcloud_.reset();
|
||||
range_.reset();
|
||||
|
||||
test_node_.reset();
|
||||
|
||||
tf_listener_.reset();
|
||||
tf_buffer_.reset();
|
||||
}
|
||||
|
||||
void Tester::createSources(const bool base_shift_correction)
|
||||
{
|
||||
// Create Scan object
|
||||
test_node_->declare_parameter(
|
||||
std::string(SCAN_NAME) + ".topic", rclcpp::ParameterValue(SCAN_TOPIC));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(SCAN_NAME) + ".topic", SCAN_TOPIC));
|
||||
|
||||
scan_ = std::make_shared<ScanWrapper>(
|
||||
test_node_, SCAN_NAME, tf_buffer_,
|
||||
BASE_FRAME_ID, GLOBAL_FRAME_ID,
|
||||
TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction);
|
||||
scan_->configure();
|
||||
|
||||
// Create PointCloud object
|
||||
test_node_->declare_parameter(
|
||||
std::string(POINTCLOUD_NAME) + ".topic", rclcpp::ParameterValue(POINTCLOUD_TOPIC));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".topic", POINTCLOUD_TOPIC));
|
||||
test_node_->declare_parameter(
|
||||
std::string(POINTCLOUD_NAME) + ".min_height", rclcpp::ParameterValue(0.1));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".min_height", 0.1));
|
||||
test_node_->declare_parameter(
|
||||
std::string(POINTCLOUD_NAME) + ".max_height", rclcpp::ParameterValue(1.0));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(POINTCLOUD_NAME) + ".max_height", 1.0));
|
||||
|
||||
pointcloud_ = std::make_shared<PointCloudWrapper>(
|
||||
test_node_, POINTCLOUD_NAME, tf_buffer_,
|
||||
BASE_FRAME_ID, GLOBAL_FRAME_ID,
|
||||
TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction);
|
||||
pointcloud_->configure();
|
||||
|
||||
// Create Range object
|
||||
test_node_->declare_parameter(
|
||||
std::string(RANGE_NAME) + ".topic", rclcpp::ParameterValue(RANGE_TOPIC));
|
||||
test_node_->set_parameter(
|
||||
rclcpp::Parameter(std::string(RANGE_NAME) + ".topic", RANGE_TOPIC));
|
||||
|
||||
test_node_->declare_parameter(
|
||||
std::string(RANGE_NAME) + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 199));
|
||||
|
||||
range_ = std::make_shared<RangeWrapper>(
|
||||
test_node_, RANGE_NAME, tf_buffer_,
|
||||
BASE_FRAME_ID, GLOBAL_FRAME_ID,
|
||||
TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction);
|
||||
range_->configure();
|
||||
}
|
||||
|
||||
void Tester::sendTransforms(const rclcpp::Time & stamp)
|
||||
{
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster =
|
||||
std::make_shared<tf2_ros::TransformBroadcaster>(test_node_);
|
||||
|
||||
geometry_msgs::msg::TransformStamped transform;
|
||||
|
||||
// base_frame -> source_frame transform
|
||||
transform.header.frame_id = BASE_FRAME_ID;
|
||||
transform.child_frame_id = SOURCE_FRAME_ID;
|
||||
|
||||
transform.header.stamp = stamp;
|
||||
transform.transform.translation.x = 0.1;
|
||||
transform.transform.translation.y = 0.1;
|
||||
transform.transform.translation.z = 0.0;
|
||||
transform.transform.rotation.x = 0.0;
|
||||
transform.transform.rotation.y = 0.0;
|
||||
transform.transform.rotation.z = 0.0;
|
||||
transform.transform.rotation.w = 1.0;
|
||||
|
||||
tf_broadcaster->sendTransform(transform);
|
||||
|
||||
// global_frame -> base_frame transform
|
||||
transform.header.frame_id = GLOBAL_FRAME_ID;
|
||||
transform.child_frame_id = BASE_FRAME_ID;
|
||||
|
||||
transform.transform.translation.x = 0.0;
|
||||
transform.transform.translation.y = 0.0;
|
||||
|
||||
tf_broadcaster->sendTransform(transform);
|
||||
}
|
||||
|
||||
bool Tester::waitScan(const std::chrono::nanoseconds & timeout)
|
||||
{
|
||||
rclcpp::Time start_time = test_node_->now();
|
||||
while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) {
|
||||
if (scan_->dataReceived()) {
|
||||
return true;
|
||||
}
|
||||
rclcpp::spin_some(test_node_->get_node_base_interface());
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Tester::waitPointCloud(const std::chrono::nanoseconds & timeout)
|
||||
{
|
||||
rclcpp::Time start_time = test_node_->now();
|
||||
while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) {
|
||||
if (pointcloud_->dataReceived()) {
|
||||
return true;
|
||||
}
|
||||
rclcpp::spin_some(test_node_->get_node_base_interface());
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Tester::waitRange(const std::chrono::nanoseconds & timeout)
|
||||
{
|
||||
rclcpp::Time start_time = test_node_->now();
|
||||
while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) {
|
||||
if (range_->dataReceived()) {
|
||||
return true;
|
||||
}
|
||||
rclcpp::spin_some(test_node_->get_node_base_interface());
|
||||
std::this_thread::sleep_for(10ms);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void Tester::checkScan(const std::vector<nav2_collision_monitor::Point> & data)
|
||||
{
|
||||
ASSERT_EQ(data.size(), 4u);
|
||||
|
||||
// Point 0: (1.0 + 0.1, 0.0 + 0.1)
|
||||
EXPECT_NEAR(data[0].x, 1.1, EPSILON);
|
||||
EXPECT_NEAR(data[0].y, 0.1, EPSILON);
|
||||
|
||||
// Point 1: (0.0 + 0.1, 1.0 + 0.1)
|
||||
EXPECT_NEAR(data[1].x, 0.1, EPSILON);
|
||||
EXPECT_NEAR(data[1].y, 1.1, EPSILON);
|
||||
|
||||
// Point 2: (-1.0 + 0.1, 0.0 + 0.1)
|
||||
EXPECT_NEAR(data[2].x, -0.9, EPSILON);
|
||||
EXPECT_NEAR(data[2].y, 0.1, EPSILON);
|
||||
|
||||
// Point 3: (0.0 + 0.1, -1.0 + 0.1)
|
||||
EXPECT_NEAR(data[3].x, 0.1, EPSILON);
|
||||
EXPECT_NEAR(data[3].y, -0.9, EPSILON);
|
||||
}
|
||||
|
||||
void Tester::checkPointCloud(const std::vector<nav2_collision_monitor::Point> & data)
|
||||
{
|
||||
ASSERT_EQ(data.size(), 2u);
|
||||
|
||||
// Point 0: (0.5 + 0.1, 0.5 + 0.1)
|
||||
EXPECT_NEAR(data[0].x, 0.6, EPSILON);
|
||||
EXPECT_NEAR(data[0].y, 0.6, EPSILON);
|
||||
|
||||
// Point 1: (-0.5 + 0.1, -0.5 + 0.1)
|
||||
EXPECT_NEAR(data[1].x, -0.4, EPSILON);
|
||||
EXPECT_NEAR(data[1].y, -0.4, EPSILON);
|
||||
|
||||
// Point 2 should be out of scope by height
|
||||
}
|
||||
|
||||
void Tester::checkRange(const std::vector<nav2_collision_monitor::Point> & data)
|
||||
{
|
||||
ASSERT_EQ(data.size(), 21u);
|
||||
|
||||
const double angle_increment = M_PI / 199;
|
||||
double angle = -M_PI / (10 * 2);
|
||||
int i;
|
||||
for (i = 0; i < 199 / 10 + 1; i++) {
|
||||
ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON);
|
||||
ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON);
|
||||
angle += angle_increment;
|
||||
}
|
||||
// Check for the latest FoW/2 point
|
||||
angle = M_PI / (10 * 2);
|
||||
ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON);
|
||||
ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testGetData)
|
||||
{
|
||||
rclcpp::Time curr_time = test_node_->now();
|
||||
|
||||
createSources();
|
||||
|
||||
sendTransforms(curr_time);
|
||||
|
||||
// Publish data for sources
|
||||
test_node_->publishScan(curr_time, 1.0);
|
||||
test_node_->publishPointCloud(curr_time);
|
||||
test_node_->publishRange(curr_time, 1.0);
|
||||
|
||||
// Wait until all sources will receive the data
|
||||
ASSERT_TRUE(waitScan(500ms));
|
||||
ASSERT_TRUE(waitPointCloud(500ms));
|
||||
ASSERT_TRUE(waitRange(500ms));
|
||||
|
||||
// Check Scan data
|
||||
std::vector<nav2_collision_monitor::Point> data;
|
||||
scan_->getData(curr_time, data);
|
||||
checkScan(data);
|
||||
|
||||
// Check Pointcloud data
|
||||
data.clear();
|
||||
pointcloud_->getData(curr_time, data);
|
||||
checkPointCloud(data);
|
||||
|
||||
// Check Range data
|
||||
data.clear();
|
||||
range_->getData(curr_time, data);
|
||||
checkRange(data);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testGetOutdatedData)
|
||||
{
|
||||
rclcpp::Time curr_time = test_node_->now();
|
||||
|
||||
createSources();
|
||||
|
||||
sendTransforms(curr_time);
|
||||
|
||||
// Publish outdated data for sources
|
||||
test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s, 1.0);
|
||||
test_node_->publishPointCloud(curr_time - DATA_TIMEOUT - 1s);
|
||||
test_node_->publishRange(curr_time - DATA_TIMEOUT - 1s, 1.0);
|
||||
|
||||
// Wait until all sources will receive the data
|
||||
ASSERT_TRUE(waitScan(500ms));
|
||||
ASSERT_TRUE(waitPointCloud(500ms));
|
||||
ASSERT_TRUE(waitRange(500ms));
|
||||
|
||||
// Scan data should be empty
|
||||
std::vector<nav2_collision_monitor::Point> data;
|
||||
scan_->getData(curr_time, data);
|
||||
ASSERT_EQ(data.size(), 0u);
|
||||
|
||||
// Pointcloud data should be empty
|
||||
pointcloud_->getData(curr_time, data);
|
||||
ASSERT_EQ(data.size(), 0u);
|
||||
|
||||
// Range data should be empty
|
||||
range_->getData(curr_time, data);
|
||||
ASSERT_EQ(data.size(), 0u);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testIncorrectFrameData)
|
||||
{
|
||||
rclcpp::Time curr_time = test_node_->now();
|
||||
|
||||
createSources();
|
||||
|
||||
// Send incorrect transform
|
||||
sendTransforms(curr_time - 1s);
|
||||
|
||||
// Publish data for sources
|
||||
test_node_->publishScan(curr_time, 1.0);
|
||||
test_node_->publishPointCloud(curr_time);
|
||||
test_node_->publishRange(curr_time, 1.0);
|
||||
|
||||
// Wait until all sources will receive the data
|
||||
ASSERT_TRUE(waitScan(500ms));
|
||||
ASSERT_TRUE(waitPointCloud(500ms));
|
||||
ASSERT_TRUE(waitRange(500ms));
|
||||
|
||||
// Scan data should be empty
|
||||
std::vector<nav2_collision_monitor::Point> data;
|
||||
scan_->getData(curr_time, data);
|
||||
ASSERT_EQ(data.size(), 0u);
|
||||
|
||||
// Pointcloud data should be empty
|
||||
pointcloud_->getData(curr_time, data);
|
||||
ASSERT_EQ(data.size(), 0u);
|
||||
|
||||
// Range data should be empty
|
||||
range_->getData(curr_time, data);
|
||||
ASSERT_EQ(data.size(), 0u);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testIncorrectData)
|
||||
{
|
||||
rclcpp::Time curr_time = test_node_->now();
|
||||
|
||||
createSources();
|
||||
|
||||
sendTransforms(curr_time);
|
||||
|
||||
// Publish data for sources
|
||||
test_node_->publishScan(curr_time, 2.0);
|
||||
test_node_->publishPointCloud(curr_time);
|
||||
test_node_->publishRange(curr_time, 2.0);
|
||||
|
||||
// Wait until all sources will receive the data
|
||||
ASSERT_TRUE(waitScan(500ms));
|
||||
ASSERT_TRUE(waitRange(500ms));
|
||||
|
||||
// Scan data should be empty
|
||||
std::vector<nav2_collision_monitor::Point> data;
|
||||
scan_->getData(curr_time, data);
|
||||
ASSERT_EQ(data.size(), 0u);
|
||||
|
||||
// Range data should be empty
|
||||
range_->getData(curr_time, data);
|
||||
ASSERT_EQ(data.size(), 0u);
|
||||
}
|
||||
|
||||
TEST_F(Tester, testIgnoreTimeShift)
|
||||
{
|
||||
rclcpp::Time curr_time = test_node_->now();
|
||||
|
||||
createSources(false);
|
||||
|
||||
// Send incorrect transform
|
||||
sendTransforms(curr_time - 1s);
|
||||
|
||||
// Publish data for sources
|
||||
test_node_->publishScan(curr_time, 1.0);
|
||||
test_node_->publishPointCloud(curr_time);
|
||||
test_node_->publishRange(curr_time, 1.0);
|
||||
|
||||
// Wait until all sources will receive the data
|
||||
ASSERT_TRUE(waitScan(500ms));
|
||||
ASSERT_TRUE(waitPointCloud(500ms));
|
||||
ASSERT_TRUE(waitRange(500ms));
|
||||
|
||||
// Scan data should be consistent
|
||||
std::vector<nav2_collision_monitor::Point> data;
|
||||
scan_->getData(curr_time, data);
|
||||
checkScan(data);
|
||||
|
||||
// Pointcloud data should be consistent
|
||||
data.clear();
|
||||
pointcloud_->getData(curr_time, data);
|
||||
checkPointCloud(data);
|
||||
|
||||
// Range data should be consistent
|
||||
data.clear();
|
||||
range_->getData(curr_time, data);
|
||||
checkRange(data);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
// Initialize the system
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
// Actual testing
|
||||
bool test_result = RUN_ALL_TESTS();
|
||||
|
||||
// Shutdown
|
||||
rclcpp::shutdown();
|
||||
|
||||
return test_result;
|
||||
}
|
||||
Reference in New Issue
Block a user