-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* add velocity based polygon * fix header, copyright and variable name change * optimise polygon update * optimise duplicated code with setPolygonShape * add warning log for uncovered speed * update feedback * rename polygon velocity to velocity polygon * cleanup * fix typo * add dynamic support for velocity polygon * wrap try catch for getting parameters * update naming and linting * use switch case * Revert "use switch case" This reverts commit 1230ede. * fix proper return for invalid parameters * remove topic parameter for velocity polygon * fix formatting manually * continue if points are not defined * rewrite velocity polygon with polygon base class Signed-off-by: nelson <kaichie.lee@gmail.com> * update review comments and description Signed-off-by: nelson <kaichie.lee@gmail.com> * add VelocityPolygon to detector node Signed-off-by: nelson <kaichie.lee@gmail.com> * review update Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> * add velocity polygon tests Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * add in-line comment Signed-off-by: nelson <kaichie.lee@gmail.com> * fix push back Signed-off-by: nelson <kaichie.lee@gmail.com> * minor change and update README Signed-off-by: nelson <kaichie.lee@gmail.com> * update README Signed-off-by: nelson <kaichie.lee@gmail.com> --------- Signed-off-by: nelson <kaichie.lee@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
- Loading branch information
1 parent
4c3174e
commit a3ed745
Showing
16 changed files
with
1,092 additions
and
10 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
115 changes: 115 additions & 0 deletions
115
nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,115 @@ | ||
// Copyright (c) 2023 Dexory | ||
// | ||
// 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__VELOCITY_POLYGON_HPP_ | ||
#define NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_ | ||
|
||
#include <memory> | ||
#include <string> | ||
#include <vector> | ||
|
||
#include "geometry_msgs/msg/polygon_stamped.hpp" | ||
#include "nav2_collision_monitor/polygon.hpp" | ||
#include "nav2_collision_monitor/types.hpp" | ||
#include "nav2_util/lifecycle_node.hpp" | ||
#include "rclcpp/rclcpp.hpp" | ||
#include "tf2_ros/buffer.h" | ||
|
||
namespace nav2_collision_monitor | ||
{ | ||
|
||
/** | ||
* @brief Velocity polygon class. | ||
* This class contains all the points of the polygon and | ||
* the expected condition of the velocity based polygon. | ||
*/ | ||
class VelocityPolygon : public Polygon | ||
{ | ||
public: | ||
/** | ||
* @brief VelocityPolygon constructor | ||
* @param node Collision Monitor node pointer | ||
* @param polygon_name Name of main polygon | ||
*/ | ||
VelocityPolygon( | ||
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 VelocityPolygon destructor | ||
*/ | ||
virtual ~VelocityPolygon(); | ||
|
||
/** | ||
* @brief Overriden getParameters function for VelocityPolygon parameters | ||
* @param polygon_sub_topic Not used in VelocityPolygon | ||
* @param polygon_pub_topic Output name of polygon publishing topic | ||
* @param footprint_topic Not used in VelocityPolygon | ||
* @return True if all parameters were obtained or false in failure case | ||
*/ | ||
bool getParameters( | ||
std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic, | ||
std::string & /*footprint_topic*/) override; | ||
|
||
/** | ||
* @brief Overriden updatePolygon function for VelocityPolygon | ||
* @param cmd_vel_in Robot twist command input | ||
*/ | ||
void updatePolygon(const Velocity & cmd_vel_in) override; | ||
|
||
protected: | ||
/** | ||
* @brief Custom struc to store the parameters of the sub-polygon | ||
* @param poly_ The points of the sub-polygon | ||
* @param velocity_polygon_name_ The name of the sub-polygon | ||
* @param linear_min_ The minimum linear velocity | ||
* @param linear_max_ The maximum linear velocity | ||
* @param theta_min_ The minimum angular velocity | ||
* @param theta_max_ The maximum angular velocity | ||
* @param direction_end_angle_ The end angle of the direction(For holonomic robot only) | ||
* @param direction_start_angle_ The start angle of the direction(For holonomic robot only) | ||
*/ | ||
struct SubPolygonParameter | ||
{ | ||
std::vector<Point> poly_; | ||
std::string velocity_polygon_name_; | ||
double linear_min_; | ||
double linear_max_; | ||
double theta_min_; | ||
double theta_max_; | ||
double direction_end_angle_; | ||
double direction_start_angle_; | ||
}; | ||
|
||
/** | ||
* @brief Check if the velocities and direction is in expected range. | ||
* @param cmd_vel_in Robot twist command input | ||
* @param sub_polygon_param Sub polygon parameters | ||
* @return True if speed and direction is within the condition | ||
*/ | ||
bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param); | ||
|
||
// Clock | ||
rclcpp::Clock::SharedPtr clock_; | ||
|
||
// Variables | ||
/// @brief Flag to indicate if the robot is holonomic | ||
bool holonomic_; | ||
/// @brief Vector to store the parameters of the sub-polygon | ||
std::vector<SubPolygonParameter> sub_polygons_; | ||
}; // class VelocityPolygon | ||
|
||
} // namespace nav2_collision_monitor | ||
|
||
#endif // NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.