HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
navigations/dwb_critics/include/dwb_critics/alignment_util.h Executable file
View File

@@ -0,0 +1,54 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_ALIGNMENT_UTIL_H
#define DWB_CRITICS_ALIGNMENT_UTIL_H
#include <geometry_msgs/Pose2D.h>
namespace dwb_critics
{
/**
* @brief Projects the given pose forward the specified distance in the x direction.
* @param pose Input pose
* @param distance distance to move (in meters)
* @return Pose distance meters in front of input pose.
*
* (used in both path_align and dist_align)
*/
geometry_msgs::Pose2D getForwardPose(const geometry_msgs::Pose2D& pose, double distance);
} // namespace dwb_critics
#endif // DWB_CRITICS_ALIGNMENT_UTIL_H

79
navigations/dwb_critics/include/dwb_critics/base_obstacle.h Executable file
View File

@@ -0,0 +1,79 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_BASE_OBSTACLE_H
#define DWB_CRITICS_BASE_OBSTACLE_H
#include <dwb_local_planner/trajectory_critic.h>
namespace dwb_critics
{
/**
* @class BaseObstacleCritic
* @brief Uses costmap to assign negative costs if a circular robot would collide at any point of the trajectory.
*
* This class can only be used to figure out if a circular robot is in collision. If the cell corresponding
* with any of the poses in the Trajectory is an obstacle, inscribed obstacle or unknown, it will return a
* negative cost. Otherwise it will return either the final pose's cost, or the sum of all poses, depending
* on the sum_scores parameter.
*
* Other classes (like ObstacleFootprintCritic) can do more advanced checking for collisions.
*/
class BaseObstacleCritic : public dwb_local_planner::TrajectoryCritic
{
public:
void onInit() override;
double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
void addCriticVisualization(sensor_msgs::PointCloud& pc) override;
/**
* @brief Return the obstacle score for a particular pose
* @param costmap Dereferenced costmap
* @param pose Pose to check
*/
virtual double scorePose(const nav_core2::Costmap& costmap, const geometry_msgs::Pose2D& pose);
/**
* @brief Check to see whether a given cell cost is valid for driving through.
* @param cost Cost of the cell
* @return Return true if valid cell
*/
virtual bool isValidCost(const unsigned char cost);
protected:
bool sum_scores_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS_BASE_OBSTACLE_H

65
navigations/dwb_critics/include/dwb_critics/goal_align.h Executable file
View File

@@ -0,0 +1,65 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_GOAL_ALIGN_H_
#define DWB_CRITICS_GOAL_ALIGN_H_
#include <dwb_critics/goal_dist.h>
#include <vector>
#include <string>
namespace dwb_critics
{
/**
* @class GoalAlignCritic
* @brief Scores trajectories based on whether the robot ends up pointing toward the eventual goal
*
* Similar to GoalDistCritic, this critic finds the pose from the global path farthest from the robot
* that is still on the costmap and then evaluates how far the front of the robot is from that point.
* This works as a proxy to calculating which way the robot should be pointing.
*/
class GoalAlignCritic: public GoalDistCritic
{
public:
GoalAlignCritic() : forward_point_distance_(0.0) {}
void onInit() override;
bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
double scorePose(const geometry_msgs::Pose2D& pose) override;
protected:
double forward_point_distance_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS_GOAL_ALIGN_H_

60
navigations/dwb_critics/include/dwb_critics/goal_dist.h Executable file
View File

@@ -0,0 +1,60 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_GOAL_DIST_H_
#define DWB_CRITICS_GOAL_DIST_H_
#include <dwb_critics/map_grid.h>
#include <vector>
namespace dwb_critics
{
/**
* @class GoalDistCritic
* @brief Scores trajectories based on how far along the global path they end up.
*
* This trajectory critic helps ensure progress along the global path. It finds the pose from the
* global path farthest from the robot that is still on the costmap, and aims for that point by
* assigning the lowest cost to the cell corresponding with that farthest pose.
*/
class GoalDistCritic: public MapGridCritic
{
public:
bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
protected:
bool getLastPoseOnCostmap(const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y);
};
} // namespace dwb_critics
#endif // DWB_CRITICS_GOAL_DIST_H_

134
navigations/dwb_critics/include/dwb_critics/map_grid.h Executable file
View File

@@ -0,0 +1,134 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_MAP_GRID_H
#define DWB_CRITICS_MAP_GRID_H
#include <dwb_local_planner/trajectory_critic.h>
#include <costmap_queue/costmap_queue.h>
#include <memory>
#include <vector>
namespace dwb_critics
{
/**
* @class MapGridCritic
* @brief breadth-first scoring of all the cells in the costmap
*
* This TrajectoryCritic assigns a score to every cell in the costmap based on
* the distance to the cell from some set of source points. The cells corresponding
* with the source points are marked with some initial score, and then every other cell
* is updated with a score based on its relation to the closest source cell, based on a
* breadth-first exploration of the cells of the costmap.
*
* This approach was chosen for computational efficiency, such that each trajectory
* need not be compared to the list of source points.
*/
class MapGridCritic: public dwb_local_planner::TrajectoryCritic
{
public:
MapGridCritic() : cell_values_(-1.0) {}
// Standard TrajectoryCritic Interface
void onInit() override;
double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
void addCriticVisualization(sensor_msgs::PointCloud& pc) override;
double getScale() const override { return costmap_->getResolution() * 0.5 * scale_; }
// Helper Functions
/**
* @brief Retrieve the score for a single pose
* @param pose The pose to score, assumed to be in the same frame as the costmap
* @return The score associated with the cell of the costmap where the pose lies
*/
virtual double scorePose(const geometry_msgs::Pose2D& pose);
/**
* @brief Retrieve the score for a particular cell of the costmap
* @param x x-coordinate within the costmap
* @param y y-coordinate within the costmap
* @return the score associated with that cell.
*/
inline double getScore(unsigned int x, unsigned int y) { return cell_values_(x, y); }
/**
* @brief Sets the score of a particular cell to the obstacle cost
* @param x X coordinate of cell
* @param y Y coordinate of cell
*/
void setAsObstacle(unsigned int x, unsigned int y);
protected:
/**
* @brief Separate modes for aggregating scores across the multiple poses in a trajectory.
*
* Last returns the score associated with the last pose in the trajectory
* Sum returns the sum of all the scores
* Product returns the product of all the (non-zero) scores
*/
enum class ScoreAggregationType {Last, Sum, Product};
/**
* @class MapGridQueue
* @brief Subclass of CostmapQueue that avoids Obstacles and Unknown Values
*/
class MapGridQueue : public costmap_queue::CostmapQueue
{
public:
MapGridQueue(nav_core2::Costmap& costmap, MapGridCritic& parent)
: costmap_queue::CostmapQueue(costmap, true), parent_(parent) {}
bool validCellToQueue(const costmap_queue::CellData& cell) override;
protected:
MapGridCritic& parent_;
};
/**
* @brief Clear the queue and set cell_values_ to the appropriate number of unreachableCellScore
*/
void reset() override;
/**
* @brief Go through the queue and set the cells to the Manhattan distance from their parents
*/
void propogateManhattanDistances();
std::shared_ptr<MapGridQueue> queue_;
nav_grid::VectorNavGrid<double> cell_values_;
double obstacle_score_, unreachable_score_; ///< Special cell_values
bool stop_on_failure_;
ScoreAggregationType aggregationType_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS_MAP_GRID_H

126
navigations/dwb_critics/include/dwb_critics/metratronik_technik.h Executable file
View File

@@ -0,0 +1,126 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef METRATRONIK_TECHNIK_H_INCLUDE_
#define METRATRONIK_TECHNIK_H_INCLUDE_
#include <dwb_local_planner/trajectory_critic.h>
#include <geometry_msgs/PoseStamped.h>
#include <string>
#include <vector>
#include <iostream>
namespace dwb_critics
{
/* data */
struct lineEquation
{
geometry_msgs::Pose2D Gi; /* 𝑮𝒊(𝒙𝒊,𝒚𝒊) */
geometry_msgs::Pose2D Gj; /* 𝑮𝒋(𝒙𝒋,𝒚𝒋) */
float a, b, c; /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */
};
/* Save data present position */
struct presentPosition
{
float Gxh, Gyh; /* 𝑮𝒋(𝒙h,𝒚h) The head position in Robot */
float Yaw_degree; /* Angle of Robot (degree) */
float Yaw_rad; /* Angle of Robot (degree) */
float error_line; /* 𝒆(𝑮,𝒍𝒊𝒏𝒆)=|𝒂𝒙+𝒃𝒚+𝒄|/sqrt(𝒂^2+𝒃^2) */
double distanceToEnd; /* The distance from robot to End point */
double distanceToStart; /* The distance from robot to End point */
};
class NoSolution : public std::exception
{
public:
const char *what() const throw()
{
return "Robot cannot calculate error line!";
}
};
class MetratronikTechnikCritic : public dwb_local_planner::TrajectoryCritic
{
public:
void onInit() override;
void reset() override;
bool prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan) override;
double scoreTrajectory(const dwb_msgs::Trajectory2D &traj) override;
protected:
/**
* @brief Assuming that this is an actual rotation when near the goal, score the trajectory.
*
* Phương pháp này (dễ dàng ghi đè) giả định rằng người phê bình đang ở giai đoạn thứ ba (như mô tả ở trên)
* và trả về một số điểm cho quỹ đạo tương ứng với độ lệch mục tiêu.
* @param traj Trajectory to score
* @return numeric score
*/
virtual double scoreMetratronikTechnik(const dwb_msgs::Trajectory2D &traj);
bool getPosition(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan, double &position);
unsigned getGoalIndex(const geometry_msgs::Pose2D &robot_pose,
const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int start_index,
unsigned int last_valid_index) const;
/**
* @brief To caculated the equation of the line
* @param start_point start point of the line
* @param end_point end point of the line
*/
bool getLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point, lineEquation &leq);
/**
* @brief Calculating error
* @param[in] Lm The distance from the center of the wheel axis to the center line
* @param[in] Yaw_degree Angle of Robot (degree)
*/
presentPosition calculateError(const lineEquation &leq, const float Lm, const geometry_msgs::Pose2D robot_pose, float pha);
bool in_window_, running_;
double sq_window_;
double xy_local_goal_tolerance_;
double xy_local_goal_tolerance_sq_; ///< Cached squared tolerance
double current_speed_, stopped_velocity_;
double slowing_factor_;
double current_position_;
double angle_threshold_;
double Lm_;
double dxy_sq_;
std::vector<geometry_msgs::Pose2D> reached_intermediate_goals_;
};
}
#endif

70
navigations/dwb_critics/include/dwb_critics/obstacle_footprint.h Executable file
View File

@@ -0,0 +1,70 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_OBSTACLE_FOOTPRINT_H
#define DWB_CRITICS_OBSTACLE_FOOTPRINT_H
#include <dwb_critics/base_obstacle.h>
#include <nav_2d_msgs/Polygon2D.h>
#include <vector>
namespace dwb_critics
{
/**
* @class ObstacleFootprintCritic
* @brief Uses costmap 2d to assign negative costs if robot footprint is in obstacle on any point of the trajectory.
*
* Internally, this technically only checks if the border of the footprint collides with anything for computational
* efficiency. This is valid if the obstacles in the local costmap are inflated.
*
* A more robust class could check every cell within the robot's footprint without inflating the obstacles,
* at some computational cost. That is left as an excercise to the reader.
*/
class ObstacleFootprintCritic : public BaseObstacleCritic
{
public:
void onInit() override;
bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
double scorePose(const nav_core2::Costmap& costmap, const geometry_msgs::Pose2D& pose) override;
virtual double scorePose(const nav_core2::Costmap& costmap, const geometry_msgs::Pose2D& pose,
const nav_2d_msgs::Polygon2D& oriented_footprint);
double getScale() const override { return costmap_->getResolution() * scale_; }
protected:
nav_2d_msgs::Polygon2D footprint_spec_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS_OBSTACLE_FOOTPRINT_H

154
navigations/dwb_critics/include/dwb_critics/oscillation.h Executable file
View File

@@ -0,0 +1,154 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_OSCILLATION_H_
#define DWB_CRITICS_OSCILLATION_H_
#include <dwb_local_planner/trajectory_critic.h>
#include <vector>
#include <string>
namespace dwb_critics
{
/**
* @class OscillationCritic
* @brief Checks to see whether the sign of the commanded velocity flips frequently
*
* This critic figures out if the commanded trajectories are oscillating by seeing
* if one of the dimensions (x,y,theta) flips from positive to negative and then back
* (or vice versa) without moving sufficiently far or waiting sufficiently long.
*
* Scenario 1: Robot moves one meter forward, and then two millimeters backward.
* Another forward motion would be considered oscillating, since the x dimension would then
* flip from positive to negative and then back to negative. Hence, when scoring different
* trajectories, positive velocity commands will get the oscillation_score (-5.0, or invalid)
* and only negative velocity commands will be considered valid.
* Scenario 2: Robot moves one meter forward, and then one meter backward.
* The robot has thus moved one meter since flipping the sign of the x direction, which
* is greater than our oscillation_reset_dist, so its not considered oscillating, so all
* trajectories are considered valid.
*
* Note: The critic will only check oscillations in the x dimension while it exceeds
* a particular value (x_only_threshold_). If it dips below that magnitude, it will
* also check for oscillations in the y and theta dimensions. If x_only_threshold_ is
* negative, then the critic will always check all dimensions.
*
* Implementation Details:
* The critic saves the robot's current position when it prepares, and what the actual
* commanded velocity was during the debrief step. Upon debriefing, if the sign of any of
* dimensions has flipped since the last command, the position is saved as prev_stationary_pose_.
*
* If the linear or angular distance from prev_stationary_pose_ to the current pose exceeds
* the limits, the oscillation flags are reset so the previous sign change is no longer remembered.
* This assumes that oscillation_reset_dist_ or oscillation_reset_angle_ are positive. Otherwise,
* it uses a time based delay reset function.
*/
class OscillationCritic: public dwb_local_planner::TrajectoryCritic
{
public:
void onInit() override;
bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
void reset() override;
void debrief(const nav_2d_msgs::Twist2D& cmd_vel) override;
protected:
/**
* @class CommandTrend
* @brief Helper class for performing the same logic on the x,y and theta dimensions
*/
class CommandTrend
{
public:
CommandTrend();
void reset();
/**
* @brief update internal flags based on the commanded velocity
* @param velocity commanded velocity for the dimension this trend is tracking
* @return true if the sign has flipped
*/
bool update(double velocity);
/**
* @brief Check to see whether the proposed velocity would be considered oscillating
* @param velocity the velocity to evaluate
* @return true if the sign has flipped more than once
*/
bool isOscillating(double velocity);
/**
* @brief Check whether we are currently tracking a flipped sign
* @return True if the sign has flipped
*/
bool hasSignFlipped();
protected:
// Simple Enum for Tracking
enum class Sign { ZERO, POSITIVE, NEGATIVE };
Sign sign_;
bool positive_only_, negative_only_;
};
/**
* @brief Given a command that has been selected, track each component's sign for oscillations
* @param cmd_vel The command velocity selected by the algorithm
* @return True if the sign on any of the components flipped
*/
bool setOscillationFlags(const nav_2d_msgs::Twist2D& cmd_vel);
/**
* @brief Return true if the robot has travelled far enough or waited long enough
*/
bool resetAvailable();
CommandTrend x_trend_, y_trend_, theta_trend_;
double oscillation_reset_dist_, oscillation_reset_angle_, x_only_threshold_;
double oscillation_reset_time_;
// Cached square parameter
double oscillation_reset_dist_sq_;
// Saved positions
geometry_msgs::Pose2D pose_, prev_stationary_pose_;
// Saved timestamp
ros::Time prev_reset_time_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS_OSCILLATION_H_

71
navigations/dwb_critics/include/dwb_critics/path_align.h Executable file
View File

@@ -0,0 +1,71 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_PATH_ALIGN_H_
#define DWB_CRITICS_PATH_ALIGN_H_
#include <dwb_critics/path_dist.h>
#include <vector>
#include <string>
namespace dwb_critics
{
/**
* @class PathAlignCritic
* @brief Scores trajectories based on how far from the global path the front of the robot ends up.
*
* This uses the costmap grid as a proxy for calculating which way the robot should be facing relative
* to the global path. Instead of scoring how far the center of the robot is away from the global path,
* this critic calculates how far a point forward_point_distance in front of the robot is from the global
* path. This biases the planner toward trajectories that line up with the global plan.
*
* When the robot is near the end of the path, the scale of this critic is set to zero. When the projected
* point is past the global goal, we no longer want this critic to try to align to a part of the global path
* that isn't there.
*/
class PathAlignCritic: public PathDistCritic
{
public:
PathAlignCritic() : zero_scale_(false), forward_point_distance_(0.0) {}
void onInit() override;
bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
double getScale() const override;
double scorePose(const geometry_msgs::Pose2D& pose) override;
protected:
bool zero_scale_;
double forward_point_distance_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS_PATH_ALIGN_H_

53
navigations/dwb_critics/include/dwb_critics/path_dist.h Executable file
View File

@@ -0,0 +1,53 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_PATH_DIST_H_
#define DWB_CRITICS_PATH_DIST_H_
#include <dwb_critics/map_grid.h>
namespace dwb_critics
{
/**
* @class PathDistCritic
* @brief Scores trajectories based on how far from the global path they end up.
*/
class PathDistCritic: public MapGridCritic
{
public:
bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
};
} // namespace dwb_critics
#endif // DWB_CRITICS_PATH_DIST_H_

65
navigations/dwb_critics/include/dwb_critics/prefer_forward.h Executable file
View File

@@ -0,0 +1,65 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_PREFER_FORWARD_H_
#define DWB_CRITICS_PREFER_FORWARD_H_
#include <dwb_local_planner/trajectory_critic.h>
#include <string>
namespace dwb_critics
{
/**
* @class PreferForwardCritic
* @brief Penalize trajectories with move backwards and/or turn too much
*
* Has three different scoring conditions:
* 1) If the trajectory's x velocity is negative, return the penalty
* 2) If the trajectory's x is low and the theta is also low, return the penalty.
* 3) Otherwise, return a scaled version of the trajectory's theta.
*/
class PreferForwardCritic: public dwb_local_planner::TrajectoryCritic
{
public:
PreferForwardCritic() : penalty_(1.0), strafe_x_(0.1), strafe_theta_(0.2), theta_scale_(10.0) {}
void onInit() override;
double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
protected:
double penalty_, strafe_x_, strafe_theta_, theta_scale_;
};
} /* namespace dwb_critics */
#endif /* DWB_CRITICS_PREFER_FORWARD_H_ */

100
navigations/dwb_critics/include/dwb_critics/rotate_to_goal.h Executable file
View File

@@ -0,0 +1,100 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_ROTATE_TO_GOAL_H_
#define DWB_CRITICS_ROTATE_TO_GOAL_H_
#include <dwb_local_planner/trajectory_critic.h>
#include <string>
#include <vector>
namespace dwb_critics
{
/**
* @class RotateToGoalCritic
* @brief Forces the commanded trajectories to only be rotations if within a certain distance window
*
* This used to be built in to the DWA Local Planner as the LatchedStopRotate controller,
* but has been moved to a critic for consistency.
*
* The critic has three distinct phases.
* 1) If the current pose is outside xy_goal_tolerance LINEAR distance from the goal pose, this critic
* will just return score 0.0.
* 2) If within the xy_goal_tolerance and the robot is still moving with non-zero linear motion, this critic
* will only allow trajectories that are slower than the current speed in order to stop the robot (within
* the robot's acceleration limits). The returned score will be the robot's linear speed squared, multiplied
* by the slowing_factor parameter (default 5.0) added to the result of scoreRotation.
* 3) If within the xy_goal_tolerance and the robot has sufficiently small linear motion, this critic will
* score trajectories that have linear movement as invalid and score the rest based on the result of the
* scoreRotation method
*
* The scoreRotation method can be overriden, but the default behavior is to return the shortest angular distance
* between the goal pose and a pose from the trajectory. Which pose depends on the lookahead_time parameter.
* * If the lookahead_time parameter is negative, the pose evaluated will be the last pose in the trajectory,
* which is the same as DWA's behavior. This is the default.
* * Otherwise, a new pose will be projected using the dwb_local_planner::projectPose. By using a lookahead
* time shorter than sim_time, the critic will be less concerned about overshooting the goal yaw and thus will
* continue to turn faster for longer.
*/
class RotateToGoalCritic : public dwb_local_planner::TrajectoryCritic
{
public:
void onInit() override;
void reset() override;
bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) override;
double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
/**
* @brief Assuming that this is an actual rotation when near the goal, score the trajectory.
*
* This (easily overridden) method assumes that the critic is in the third phase (as described above)
* and returns a numeric score for the trajectory relative to the goal yaw.
* @param traj Trajectory to score
* @return numeric score
*/
virtual double scoreRotation(const dwb_msgs::Trajectory2D& traj);
protected:
bool in_window_, rotating_;
double goal_yaw_;
double xy_goal_tolerance_;
double xy_goal_tolerance_sq_; ///< Cached squared tolerance
double current_xy_speed_sq_, stopped_xy_velocity_sq_;
double slowing_factor_;
double lookahead_time_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS_ROTATE_TO_GOAL_H_

59
navigations/dwb_critics/include/dwb_critics/twirling.h Executable file
View File

@@ -0,0 +1,59 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS_TWIRLING_H
#define DWB_CRITICS_TWIRLING_H
#include <dwb_local_planner/trajectory_critic.h>
namespace dwb_critics
{
/**
* @class TwirlingCritic
* @brief Penalize trajectories with rotational velocities
*
* This class provides a cost based on how much a robot "twirls" on its way to the goal. With
* differential-drive robots, there isn't a choice, but with holonomic or near-holonomic robots,
* sometimes a robot spins more than you'd like on its way to a goal. This class provides a way
* to assign a penalty purely to rotational velocities.
*/
class TwirlingCritic: public dwb_local_planner::TrajectoryCritic
{
public:
void onInit() override;
double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override;
};
} // namespace dwb_critics
#endif // DWB_CRITICS_TWIRLING_H