Skip to content

Commit

Permalink
Merge pull request ros-navigation#20 from botsandus/AUTO-795_backport…
Browse files Browse the repository at this point in the history
…_nav2_main

AUTO-795 Backport nav2 main features
  • Loading branch information
doisyg authored Apr 21, 2023
2 parents ac269ff + ae20a92 commit 7270c74
Show file tree
Hide file tree
Showing 28 changed files with 775 additions and 160 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -129,13 +129,16 @@ class CollisionMonitor : public nav2_util::LifecycleNode
* 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 rclcpp::Duration & source_timeout,
const bool base_shift_correction);

/**
* @brief Main processing routine
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@ class PointCloud : public Source
* @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,
Expand All @@ -49,7 +51,8 @@ class PointCloud : public Source
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
const rclcpp::Duration & source_timeout);
const rclcpp::Duration & source_timeout,
const bool base_shift_correction);
/**
* @brief PointCloud destructor
*/
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -86,10 +86,10 @@ class Polygon
*/
ActionType getActionType() 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
* @brief Obtains polygon minimum points to enter inside polygon causing the action
* @return Minimum number of data readings within a zone to trigger the action
*/
int getMaxPoints() const;
int getMinPoints() const;
/**
* @brief Obtains speed slowdown ratio for current polygon.
* Applicable for SLOWDOWN model.
Expand Down Expand Up @@ -198,8 +198,8 @@ class 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 Minimum number of data readings within a zone to trigger the action
int min_points_;
/// @brief Robot slowdown (share of its actual speed)
double slowdown_ratio_;
/// @brief Time before collision in seconds
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@ class Range : public Source
* @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,
Expand All @@ -49,7 +51,8 @@ class Range : public Source
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
const rclcpp::Duration & source_timeout);
const rclcpp::Duration & source_timeout,
const bool base_shift_correction);
/**
* @brief Range destructor
*/
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@ class Scan : public Source
* @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,
Expand All @@ -49,7 +51,8 @@ class Scan : public Source
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
const rclcpp::Duration & source_timeout);
const rclcpp::Duration & source_timeout,
const bool base_shift_correction);
/**
* @brief Scan destructor
*/
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@ class Source
* @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,
Expand All @@ -54,7 +56,8 @@ class Source
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
const rclcpp::Duration & source_timeout);
const rclcpp::Duration & source_timeout,
const bool base_shift_correction);
/**
* @brief Source destructor
*/
Expand Down Expand Up @@ -110,6 +113,9 @@ class Source
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_;
}; // class Source

} // namespace nav2_collision_monitor
Expand Down
2 changes: 1 addition & 1 deletion nav2_collision_monitor/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?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.5</version>
<version>1.0.0</version>
<description>Collision Monitor</description>
<maintainer email="[email protected]">Alexey Merzlyakov</maintainer>
<maintainer email="[email protected]">Steve Macenski</maintainer>
Expand Down
7 changes: 4 additions & 3 deletions nav2_collision_monitor/params/collision_monitor_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ collision_monitor:
state_topic: "collision_monitor_state"
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.
Expand All @@ -18,14 +19,14 @@ collision_monitor:
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
min_points: 4
visualize: True
polygon_pub_topic: "polygon_stop"
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
min_points: 4
slowdown_ratio: 0.3
visualize: True
polygon_pub_topic: "polygon_slowdown"
Expand All @@ -35,7 +36,7 @@ collision_monitor:
footprint_topic: "/local_costmap/published_footprint"
time_before_collision: 2.0
simulation_time_step: 0.1
max_points: 5
min_points: 6
visualize: False
observation_sources: ["scan"]
scan:
Expand Down
20 changes: 14 additions & 6 deletions nav2_collision_monitor/src/collision_monitor_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -221,6 +221,10 @@ bool CollisionMonitor::getParameters(
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));
Expand All @@ -231,7 +235,10 @@ bool CollisionMonitor::getParameters(
return false;
}

if (!configureSources(base_frame_id, odom_frame_id, transform_tolerance, source_timeout)) {
if (
!configureSources(
base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction))
{
return false;
}

Expand Down Expand Up @@ -287,7 +294,8 @@ 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 rclcpp::Duration & source_timeout,
const bool base_shift_correction)
{
try {
auto node = shared_from_this();
Expand All @@ -305,23 +313,23 @@ bool CollisionMonitor::configureSources(
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);
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);
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);
transform_tolerance, source_timeout, base_shift_correction);

r->configure();

Expand Down Expand Up @@ -409,7 +417,7 @@ bool CollisionMonitor::processStopSlowdown(
return false;
}

if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) {
if (polygon->getPointsInside(collision_points) >= polygon->getMinPoints()) {
if (polygon->getActionType() == STOP) {
// Setting up zero velocity for STOP model
robot_action.polygon_name = polygon->getName();
Expand Down
36 changes: 25 additions & 11 deletions nav2_collision_monitor/src/pointcloud.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,11 @@ PointCloud::PointCloud(
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
const rclcpp::Duration & source_timeout)
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),
transform_tolerance, source_timeout, base_shift_correction),
data_(nullptr)
{
RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str());
Expand Down Expand Up @@ -76,16 +77,29 @@ void PointCloud::getData(
return;
}

// Obtaining the transform to get data from source frame and time where it was received
// to the base frame and current time
tf2::Transform tf_transform;
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;
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");
Expand Down
23 changes: 18 additions & 5 deletions nav2_collision_monitor/src/polygon.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -128,9 +128,9 @@ ActionType Polygon::getActionType() const
return action_type_;
}

int Polygon::getMaxPoints() const
int Polygon::getMinPoints() const
{
return max_points_;
return min_points_;
}

double Polygon::getSlowdownRatio() const
Expand Down Expand Up @@ -212,7 +212,7 @@ double Polygon::getCollisionTime(
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_) {
if (getPointsInside(points_transformed) >= min_points_) {
return time;
}
}
Expand Down Expand Up @@ -264,8 +264,21 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
}

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();
node, polygon_name_ + ".min_points", rclcpp::ParameterValue(4));
min_points_ = node->get_parameter(polygon_name_ + ".min_points").as_int();

try {
nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".max_points", rclcpp::PARAMETER_INTEGER);
min_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int() + 1;
RCLCPP_WARN(
logger_,
"[%s]: \"max_points\" parameter was deprecated. Use \"min_points\" instead to specify "
"the minimum number of data readings within a zone to trigger the action",
polygon_name_.c_str());
} catch (const std::exception &) {
// This is normal situation: max_points parameter should not being declared
}

if (action_type_ == SLOWDOWN) {
nav2_util::declare_parameter_if_not_declared(
Expand Down
36 changes: 25 additions & 11 deletions nav2_collision_monitor/src/range.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,11 @@ Range::Range(
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
const rclcpp::Duration & source_timeout)
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),
transform_tolerance, source_timeout, base_shift_correction),
data_(nullptr)
{
RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str());
Expand Down Expand Up @@ -85,16 +86,29 @@ void Range::getData(
return;
}

// Obtaining the transform to get data from source frame and time where it was received
// to the base frame and current time
tf2::Transform tf_transform;
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;
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
Expand Down
Loading

0 comments on commit 7270c74

Please sign in to comment.