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

64
navigations/dwb_critics/CMakeLists.txt Executable file
View File

@@ -0,0 +1,64 @@
cmake_minimum_required(VERSION 3.0.2)
project(dwb_critics)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall;-Werror")
find_package(catkin REQUIRED COMPONENTS
angles
costmap_queue
dwb_local_planner
geometry_msgs
nav_2d_msgs
nav_2d_utils
nav_core2
nav_grid_iterators
pluginlib
roscpp
sensor_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS angles costmap_queue dwb_local_planner geometry_msgs nav_2d_msgs nav_2d_utils nav_core2 nav_grid_iterators pluginlib roscpp sensor_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME}
src/alignment_util.cpp
src/map_grid.cpp
src/goal_dist.cpp
src/path_dist.cpp
src/goal_align.cpp
src/path_align.cpp
src/base_obstacle.cpp
src/obstacle_footprint.cpp
src/oscillation.cpp
src/prefer_forward.cpp
src/rotate_to_goal.cpp
src/twirling.cpp
)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
if(CATKIN_ENABLE_TESTING)
find_package(roslint REQUIRED)
roslint_cpp()
roslint_add_test()
endif()
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(FILES default_critics.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

31
navigations/dwb_critics/README.md Executable file
View File

@@ -0,0 +1,31 @@
# dwb_critics
This package contains plugin implementations of `dwb_local_planner::TrajectoryCritic` sufficient to replicate the behavior of `dwa_local_planner`.
## Obstacle Avoidance
There are two options for critics that examine the contents of the costmap and reject trajectories that come close to obstacles.
* `BaseObstacleCritic` assumes a circular robot, and therefore only needs to check one cell in the costmap for each pose in the trajectory (assuming the costmap is properly inflated).
* `ObstacleFootprintCritic` uses the robot's footprint and checks all of the cells along the outline of the robot's footprint at each pose.
## Progress Toward the Goal along the Path
There are two critics which evaluate the robot's position at the end of the trajectory relative to the goal pose and the global plan.
* `GoalDistCritic` estimates the distance from the last pose to the goal pose.
* `PathDistCritic` estimates the distance from the last pose to the closest pose in the global plan.
## Alignment
There are also two critics for keeping the robot pointed in the right direction. They use a point on the front of the robot as a proxy to calculate which way the robot is pointed.
* `GoalAlignCritic` estimates the distance from the front of the robot to the goal pose. This score will be higher if the robot is pointed away from the goal.
* `PathAlignCritic` estimates the distance from the front of the robot to the closest point in the global plan. This score will be higher if the robot is pointed away from the global plan.
## Rotating to the Goal
There is a special case in the navigation when the robot reaches the correct XY position, but still needs to rotate to the proper yaw. The standard critics will not be useful in this case. Instead we have `RotateToGoalCritic` which operates in a couple different modes.
* If the robot is not yet in the correct XY position, it has no effect on the trajectory score.
* If the robot is at the correct XY position but still moving, this critic will score the trajecotries such that the robot slows down.
* If the robot is at the correct XY position and stopped its forward motion, the critic will
A) Disallow trajectories with forward motion
B) Score trajectories (rotations) based on how close to the goal yaw they get.
## Other Critics
* `OscillationCritic` detects oscillations by looking at the sign of the commanded motions. For example, if in a short window, the robot moves forward and then backward, it will penalize further trajectories that move forward again, as that is considered an oscillation.
* `PreferForwardCritic` was implemented but not used by `DWA` and penalize trajectories that move backwards and/or turn too much.
* `TwirlingCritic` penalizes trajectories with rotational velocities

42
navigations/dwb_critics/default_critics.xml Executable file
View File

@@ -0,0 +1,42 @@
<class_libraries>
<library path="lib/libdwb_critics">
<class type="dwb_critics::PreferForwardCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Penalize trajectories with move backwards and/or turn too much</description>
</class>
<class type="dwb_critics::GoalDistCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Scores trajectories based on how far along the global path they end up.</description>
</class>
<class type="dwb_critics::PathAlignCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Scores trajectories based on how far from the global path the front of the robot ends up.
</description>
</class>
<class type="dwb_critics::GoalAlignCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Scores trajectories based on whether the robot ends up pointing toward the eventual goal
</description>
</class>
<class type="dwb_critics::PathDistCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Scores trajectories based on how far from the global path they end up.</description>
</class>
<class type="dwb_critics::OscillationCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Checks to see whether the sign of the commanded velocity flips frequently</description>
</class>
<class type="dwb_critics::RotateToGoalCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Forces the commanded trajectories to only be rotations if within a certain distance window
</description>
</class>
<class type="dwb_critics::BaseObstacleCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Uses costmap 2d to assign negative costs if a circular robot
would collide at any point of the trajectory.
</description>
</class>
<class type="dwb_critics::ObstacleFootprintCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Uses costmap 2d to assign negative costs if robot footprint is in obstacle
on any point of the trajectory.
</description>
</class>
<class type="dwb_critics::TwirlingCritic" base_class_type="dwb_local_planner::TrajectoryCritic">
<description>Penalize trajectories with rotational velocities
</description>
</class>
</library>
</class_libraries>

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

30
navigations/dwb_critics/package.xml Executable file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<package format="2">
<name>dwb_critics</name>
<version>0.3.0</version>
<description>
Implementations for dwb_local_planner TrajectoryCritic interface
</description>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<author email="davidvlu@gmail.com">David V. Lu!!</author>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>angles</depend>
<depend>costmap_queue</depend>
<depend>dwb_local_planner</depend>
<depend>geometry_msgs</depend>
<depend>nav_2d_msgs</depend>
<depend>nav_2d_utils</depend>
<depend>nav_core2</depend>
<depend>nav_grid_iterators</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<test_depend>roslint</test_depend>
<export>
<dwb_local_planner plugin="${prefix}/default_critics.xml"/>
</export>
</package>

47
navigations/dwb_critics/src/alignment_util.cpp Executable file
View File

@@ -0,0 +1,47 @@
/*
* 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.
*/
#include <dwb_critics/alignment_util.h>
namespace dwb_critics
{
geometry_msgs::Pose2D getForwardPose(const geometry_msgs::Pose2D& pose, double distance)
{
geometry_msgs::Pose2D forward_pose;
forward_pose.x = pose.x + distance * cos(pose.theta);
forward_pose.y = pose.y + distance * sin(pose.theta);
forward_pose.theta = pose.theta;
return forward_pose;
}
} // namespace dwb_critics

102
navigations/dwb_critics/src/base_obstacle.cpp Executable file
View File

@@ -0,0 +1,102 @@
/*
* 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.
*/
#include <dwb_critics/base_obstacle.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(dwb_critics::BaseObstacleCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
void BaseObstacleCritic::onInit()
{
critic_nh_.param("sum_scores", sum_scores_, false);
}
double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
{
const nav_core2::Costmap& costmap = *costmap_;
double score = 0.0;
for (unsigned int i = 0; i < traj.poses.size(); ++i)
{
double pose_score = scorePose(costmap, traj.poses[i]);
// Optimized/branchless version of if (sum_scores_) score += pose_score, else score = pose_score;
score = static_cast<double>(sum_scores_) * score + pose_score;
}
return score;
}
double BaseObstacleCritic::scorePose(const nav_core2::Costmap& costmap, const geometry_msgs::Pose2D& pose)
{
unsigned int cell_x, cell_y;
if (!worldToGridBounded(costmap.getInfo(), pose.x, pose.y, cell_x, cell_y))
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
unsigned char cost = costmap(cell_x, cell_y);
if (!isValidCost(cost))
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
return cost;
}
bool BaseObstacleCritic::isValidCost(const unsigned char cost)
{
return cost != costmap_->LETHAL_OBSTACLE &&
cost != costmap_->INSCRIBED_INFLATED_OBSTACLE &&
cost != costmap_->NO_INFORMATION;
}
void BaseObstacleCritic::addCriticVisualization(sensor_msgs::PointCloud& pc)
{
sensor_msgs::ChannelFloat32 grid_scores;
grid_scores.name = name_;
const nav_core2::Costmap& costmap = *costmap_;
unsigned int size_x = costmap.getWidth();
unsigned int size_y = costmap.getHeight();
grid_scores.values.resize(size_x * size_y);
unsigned int i = 0;
for (unsigned int cy = 0; cy < size_y; cy++)
{
for (unsigned int cx = 0; cx < size_x; cx++)
{
grid_scores.values[i] = costmap(cx, cy);
i++;
}
}
pc.channels.push_back(grid_scores);
}
} // namespace dwb_critics

76
navigations/dwb_critics/src/goal_align.cpp Executable file
View File

@@ -0,0 +1,76 @@
/*
* 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.
*/
#include <dwb_critics/goal_align.h>
#include <dwb_critics/alignment_util.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/parameters.h>
#include <vector>
#include <string>
namespace dwb_critics
{
void GoalAlignCritic::onInit()
{
GoalDistCritic::onInit();
stop_on_failure_ = false;
forward_point_distance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "forward_point_distance", 0.325);
}
bool GoalAlignCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan)
{
// we want the robot nose to be drawn to its final position
// (before robot turns towards goal orientation), not the end of the
// path for the robot center. Choosing the final position after
// turning towards goal orientation causes instability when the
// robot needs to make a 180 degree turn at the end
double angle_to_goal = atan2(goal.y - pose.y, goal.x - pose.x);
nav_2d_msgs::Path2D target_poses = global_plan;
target_poses.poses.back().x += forward_point_distance_ * cos(angle_to_goal);
target_poses.poses.back().y += forward_point_distance_ * sin(angle_to_goal);
return GoalDistCritic::prepare(pose, vel, goal, target_poses);
}
double GoalAlignCritic::scorePose(const geometry_msgs::Pose2D& pose)
{
return GoalDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
}
} // namespace dwb_critics
PLUGINLIB_EXPORT_CLASS(dwb_critics::GoalAlignCritic, dwb_local_planner::TrajectoryCritic)

104
navigations/dwb_critics/src/goal_dist.cpp Executable file
View File

@@ -0,0 +1,104 @@
/*
* 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.
*/
#include <dwb_critics/goal_dist.h>
#include <nav_grid/coordinate_conversion.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <vector>
namespace dwb_critics
{
bool GoalDistCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan)
{
reset();
unsigned int local_goal_x, local_goal_y;
if (!getLastPoseOnCostmap(global_plan, local_goal_x, local_goal_y))
{
return false;
}
// Enqueue just the last pose
cell_values_.setValue(local_goal_x, local_goal_y, 0.0);
queue_->enqueueCell(local_goal_x, local_goal_y);
propogateManhattanDistances();
return true;
}
bool GoalDistCritic::getLastPoseOnCostmap(const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y)
{
const nav_core2::Costmap& costmap = *costmap_;
const nav_grid::NavGridInfo& info = costmap.getInfo();
nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution);
bool started_path = false;
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan.poses.size(); ++i)
{
double g_x = adjusted_global_plan.poses[i].x;
double g_y = adjusted_global_plan.poses[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION)
{
// Still on the costmap. Continue.
x = map_x;
y = map_y;
started_path = true;
}
else if (started_path)
{
// Off the costmap after being on the costmap. Return the last saved indices.
return true;
}
// else, we have not yet found a point on the costmap, so we just continue
}
if (started_path)
{
return true;
}
else
{
ROS_ERROR_NAMED("GoalDistCritic", "None of the points of the global plan were in the local costmap.");
return false;
}
}
} // namespace dwb_critics
PLUGINLIB_EXPORT_CLASS(dwb_critics::GoalDistCritic, dwb_local_planner::TrajectoryCritic)

202
navigations/dwb_critics/src/map_grid.cpp Executable file
View File

@@ -0,0 +1,202 @@
/*
* 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.
*/
#include <dwb_critics/map_grid.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_core2/exceptions.h>
#include <algorithm>
#include <memory>
#include <string>
namespace dwb_critics
{
// Customization of the CostmapQueue validCellToQueue method
bool MapGridCritic::MapGridQueue::validCellToQueue(const costmap_queue::CellData& cell)
{
unsigned char cost = costmap_(cell.x_, cell.y_);
if (cost == costmap_.LETHAL_OBSTACLE ||
cost == costmap_.INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_.NO_INFORMATION)
{
parent_.setAsObstacle(cell.x_, cell.y_);
return false;
}
return true;
}
void MapGridCritic::onInit()
{
queue_ = std::make_shared<MapGridQueue>(*costmap_, *this);
// Always set to true, but can be overriden by subclasses
stop_on_failure_ = true;
std::string aggro_str;
critic_nh_.param("aggregation_type", aggro_str, std::string("last"));
std::transform(aggro_str.begin(), aggro_str.end(), aggro_str.begin(), ::tolower);
if (aggro_str == "last")
{
aggregationType_ = ScoreAggregationType::Last;
}
else if (aggro_str == "sum")
{
aggregationType_ = ScoreAggregationType::Sum;
}
else if (aggro_str == "product")
{
aggregationType_ = ScoreAggregationType::Product;
}
else
{
ROS_ERROR_NAMED("MapGridCritic", "aggregation_type parameter \"%s\" invalid. Using Last.", aggro_str.c_str());
aggregationType_ = ScoreAggregationType::Last;
}
}
void MapGridCritic::setAsObstacle(unsigned int x, unsigned int y)
{
cell_values_.setValue(x, y, obstacle_score_);
}
void MapGridCritic::reset()
{
queue_->reset();
if (costmap_->getInfo() == cell_values_.getInfo())
{
cell_values_.reset();
}
else
{
obstacle_score_ = static_cast<double>(costmap_->getWidth() * costmap_->getHeight());
unreachable_score_ = obstacle_score_ + 1.0;
cell_values_.setDefaultValue(unreachable_score_);
cell_values_.setInfo(costmap_->getInfo());
}
}
void MapGridCritic::propogateManhattanDistances()
{
while (!queue_->isEmpty())
{
costmap_queue::CellData cell = queue_->getNextCell();
cell_values_.setValue(cell.x_, cell.y_,
std::abs(static_cast<int>(cell.src_x_) - static_cast<int>(cell.x_))
+ std::abs(static_cast<int>(cell.src_y_) - static_cast<int>(cell.y_)));
}
}
double MapGridCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
{
double score = 0.0;
unsigned int start_index = 0;
if (aggregationType_ == ScoreAggregationType::Product)
{
score = 1.0;
}
else if (aggregationType_ == ScoreAggregationType::Last && !stop_on_failure_)
{
start_index = traj.poses.size() - 1;
}
double grid_dist;
for (unsigned int i = start_index; i < traj.poses.size(); ++i)
{
grid_dist = scorePose(traj.poses[i]);
if (stop_on_failure_)
{
if (grid_dist == obstacle_score_)
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
}
else if (grid_dist == unreachable_score_)
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Unreachable Area.");
}
}
switch (aggregationType_)
{
case ScoreAggregationType::Last:
score = grid_dist;
break;
case ScoreAggregationType::Sum:
score += grid_dist;
break;
case ScoreAggregationType::Product:
if (score > 0)
{
score *= grid_dist;
}
break;
}
}
return score;
}
double MapGridCritic::scorePose(const geometry_msgs::Pose2D& pose)
{
unsigned int cell_x, cell_y;
// we won't allow trajectories that go off the map... shouldn't happen that often anyways
if (!worldToGridBounded(costmap_->getInfo(), pose.x, pose.y, cell_x, cell_y))
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
}
return getScore(cell_x, cell_y);
}
void MapGridCritic::addCriticVisualization(sensor_msgs::PointCloud& pc)
{
sensor_msgs::ChannelFloat32 grid_scores;
grid_scores.name = name_;
const nav_core2::Costmap& costmap = *costmap_;
unsigned int size_x = costmap.getWidth();
unsigned int size_y = costmap.getHeight();
grid_scores.values.resize(size_x * size_y);
unsigned int i = 0;
for (unsigned int cy = 0; cy < size_y; cy++)
{
for (unsigned int cx = 0; cx < size_x; cx++)
{
grid_scores.values[i] = getScore(cx, cy);
i++;
}
}
pc.channels.push_back(grid_scores);
}
} // namespace dwb_critics

362
navigations/dwb_critics/src/metratronik_technik.cpp Executable file
View File

@@ -0,0 +1,362 @@
/*
* 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.
*/
#include <dwb_critics/metratronik_technik.h>
#include <nav_2d_utils/parameters.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
const double EPSILON = 1E-5;
PLUGINLIB_EXPORT_CLASS(dwb_critics::MetratronikTechnikCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
inline double hypot_sq(double dx, double dy)
{
return dx * dx + dy * dy;
}
void MetratronikTechnikCritic::onInit()
{
xy_local_goal_tolerance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "xy_goal_tolerance", 0.25);
xy_local_goal_tolerance_sq_ = xy_local_goal_tolerance_ * xy_local_goal_tolerance_;
stopped_velocity_ = nav_2d_utils::searchAndGetParam(critic_nh_, "trans_stopped_velocity", 0.25);
critic_nh_.param("sq_window", sq_window_, 1.0);
critic_nh_.param("slowing_factor", slowing_factor_, 5.0);
critic_nh_.param("angle_threshold", angle_threshold_, 0.436332313);
critic_nh_.param("Lm", Lm_, 0.436332313);
reset();
}
void MetratronikTechnikCritic::reset()
{
in_window_ = false;
running_ = false;
}
bool MetratronikTechnikCritic::prepare(const geometry_msgs::Pose2D &pose, const nav_2d_msgs::Twist2D &vel,
const geometry_msgs::Pose2D &goal, const nav_2d_msgs::Path2D &global_plan)
{
if (!getPosition(pose, global_plan, current_position_))
{
return false;
}
// ROS_INFO("current_position_");
dxy_sq_ = hypot_sq(pose.x - goal.x, pose.y - goal.y);
in_window_ = in_window_ || dxy_sq_ <= sq_window_;
current_speed_ = vel.x;
running_ = running_ || (in_window_ && vel.x > 0.0 && vel.x <= stopped_velocity_);
return true;
}
double MetratronikTechnikCritic::scoreTrajectory(const dwb_msgs::Trajectory2D &traj)
{
if (!in_window_)
{
return 0.0;
}
else if (!running_)
{
return fabs(slowing_factor_ + scoreMetratronikTechnik(traj));
}
// If we're sufficiently close to the goal, any transforming velocity is invalid
if (fabs(traj.velocity.x) > stopped_velocity_ || fabs(traj.velocity.y) > stopped_velocity_)
{
return fabs(traj.velocity.x + slowing_factor_ + scoreMetratronikTechnik(traj));
}
return 0.01 * scoreMetratronikTechnik(traj);
}
double MetratronikTechnikCritic::scoreMetratronikTechnik(const dwb_msgs::Trajectory2D &traj)
{
if (traj.poses.empty())
{
throw nav_core2::IllegalTrajectoryException(name_, "Empty trajectory.");
}
double k_reduce_v = cos(((M_PI / 2) / Lm_) * current_position_);
double result_linear_x = traj.velocity.x * k_reduce_v;
double k_reduce_w_v = cos(((M_PI / 2) / stopped_velocity_) * result_linear_x);
double result_angular_z = atan(current_position_ / Lm_) * fabs(k_reduce_w_v);
double running_time = traj.time_offsets.back().toSec();
double score_linear_x = fabs(traj.velocity.x * running_time - sqrt(dxy_sq_));
// ROS_INFO("%f %f %f %f", running_time,fabs(traj.velocity.x*running_time), sqrt(dxy_sq_), score_linear_x);
double score_angular_z = fabs(traj.velocity.theta - result_angular_z);
// ROS_INFO("%f %f %f %f %f %f %f", result_linear_x, traj.velocity.x, result_angular_z, traj.velocity.theta, score_linear_x, score_angular_z, score_angular_z + score_linear_x);
return score_angular_z + score_linear_x;
}
bool MetratronikTechnikCritic::getPosition(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Path2D &global_plan, double &position)
{
const nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
if (global_plan.poses.empty())
{
ROS_ERROR_NAMED("MetratronikTechnikCritic", "The global plan was empty.");
return false;
}
// Chuyển đổi dữ liệu kế hoạch đường đi 'nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
std::vector<geometry_msgs::Pose2D> plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses;
// unsigned int start_index = 0;
// double distance_to_start = std::numeric_limits<double>::infinity();
// bool started_path = false;
// for (unsigned int i = 0; i < plan.size(); i++)
// {
// double g_x = plan[i].x;
// double g_y = plan[i].y;
// unsigned int map_x, map_y;
// if (worldToGridBounded(info, g_x, g_y, map_x, map_y) // chuyển tọa độ từ đơn vị mét sang pixel
// && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
// { // chi phí phải khác -1
// // Still on the costmap. Continue.
// double distance = nav_2d_utils::poseDistance(plan[i], robot_pose); // Tính khoảng cách từ vị trí của robot đến pose đang xét
// if (distance_to_start > distance)
// {
// start_index = i;
// distance_to_start = distance;
// started_path = true;
// }
// else
// {
// // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's
// // even closer to the robot, but then we would skip over parts of the plan.
// break;
// }
// }
// else if (started_path)
// {
// // Off the costmap after being on the costmap.
// break;
// }
// // else, we have not yet found a point on the costmap, so we just continue
// }
// // Nếu khồng tìm được điểm gần với robot nhất thì trả về false
// if (!started_path)
// {
// ROS_ERROR_NAMED("MetratronikTechnikCritic", "None of the points of the global plan were in the local costmap.");
// return false;
// }
// // current_index_ = start_index;
// // find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map
// // Tìm điểm pose cuôi cùng sau điểm pose bắt đầu và có tồn tại trong local map
// unsigned int last_valid_index = start_index;
// for (unsigned int i = start_index + 1; i < plan.size(); i++)
// {
// double g_x = plan[i].x;
// double g_y = plan[i].y;
// unsigned int map_x, map_y;
// if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
// {
// // Still on the costmap. Continue.
// last_valid_index = i;
// }
// else
// {
// // Off the costmap after being on the costmap.
// break;
// }
// }
// // find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point,
// // i.e. is within angle_threshold_ of the starting direction.
// // Tìm điểm mục tiêu chạy từ điểm start pose đến last valid pose dọc thep plan
// unsigned int goal_index = start_index;
// while (goal_index < last_valid_index && critic_nh_.ok())
// {
// // Tìm kiếm vị trí điểm mục tiêu phù hợp
// goal_index = getGoalIndex(robot_pose, plan, start_index, last_valid_index);
// // check if goal already reached
// bool goal_already_reached = false;
// for (auto &reached_intermediate_goal : reached_intermediate_goals_)
// {
// double distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]);
// if (distance < xy_local_goal_tolerance_)
// {
// goal_already_reached = true;
// // choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again
// // (start_index is now > goal_index)
// for (start_index = goal_index; start_index <= last_valid_index; ++start_index)
// {
// distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]);
// if (distance >= xy_local_goal_tolerance_)
// break;
// }
// break;
// }
// }
// if (!goal_already_reached)
// {
// // new goal not in reached_intermediate_goals_
// double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose);
// if (distance < xy_local_goal_tolerance_)
// {
// // the robot has currently reached the goal
// reached_intermediate_goals_.push_back(plan[goal_index]);
// ROS_DEBUG_NAMED("MetratronikTechnikCritic", "Number of reached_intermediate goals: %zu", reached_intermediate_goals_.size());
// }
// else
// {
// // we've found a new goal!
// break;
// }
// }
// }
// if (start_index >= goal_index)
// return false;
lineEquation leq;
getLine(plan.front(), plan.back(), leq);
position = calculateError(leq, Lm_, robot_pose, 0).error_line;
return true;
}
unsigned int MetratronikTechnikCritic::getGoalIndex(const geometry_msgs::Pose2D &robot_pose,
const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int start_index,
unsigned int last_valid_index) const
{
if (start_index >= last_valid_index)
return last_valid_index;
unsigned int goal_index = start_index;
for (unsigned int i = start_index + 1; i <= last_valid_index; ++i)
{
const double current_direction_x = plan[i].x - plan[i - 1].x;
const double current_direction_y = plan[i].y - plan[i - 1].y;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
{
double current_angle = atan2(current_direction_y, current_direction_x);
if (fabs(remainder(robot_pose.theta - current_angle, 2 * M_PI)) > angle_threshold_)
break;
goal_index = i;
}
}
if (nav_2d_utils::poseDistance(plan[goal_index], plan[last_valid_index]) <= xy_local_goal_tolerance_)
goal_index = last_valid_index;
return goal_index;
}
bool MetratronikTechnikCritic::getLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point, lineEquation &leq)
{
/**
* Phương trình đường thẳng có dạng:
* (y - yi)/(yj - yi) = (x - xi)/(xj - xi)
*/
leq.Gi = start_point;
leq.Gj = end_point;
leq.a = (leq.Gj.y - leq.Gi.y);
leq.b = (leq.Gi.x - leq.Gj.x);
leq.c = (leq.Gj.x * leq.Gi.y - leq.Gi.x * leq.Gj.y);
return leq.a != 0 || leq.b != 0;
}
presentPosition MetratronikTechnikCritic::calculateError(const lineEquation &leq, const float Lm, const geometry_msgs::Pose2D robot_pose, float pha)
{
presentPosition present_pose;
float Yaw_rad = robot_pose.theta;
float Yaw = ((Yaw_rad * 180) / M_PI);
Yaw_rad = (Yaw - pha) * M_PI / 180;
/*
* Tính tọa độ điểm R
*/
float G_xr = float(robot_pose.x);
float G_yr = float(robot_pose.y);
/*
* Tính tọa độ điểm H
*/
present_pose.Gxh = G_xr + Lm * cos(Yaw_rad);
present_pose.Gyh = G_yr + Lm * sin(Yaw_rad);
/**
* Phương trình đường thẳng (rh) có dạng:
* (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
*/
float A_rh = (present_pose.Gyh - G_yr);
float B_rh = (G_xr - present_pose.Gxh);
float C_rh = (present_pose.Gxh * G_yr - G_xr * present_pose.Gyh);
/*
* Phương trình đường thẳng vuông góc (y - yr)/(yh - yr) = (x - xr)/(xh - xr)
* và đi qua Gh có dạng:
* -B_rh*(x - Gxh) + Arh*(y - Gyh) = 0
*/
float A_he = -B_rh;
float B_he = A_rh;
float C_he = (-B_rh) * (-present_pose.Gxh) + A_rh * (-present_pose.Gyh);
/*
* Đường thẳng (he) giao với đường thẳng (ij) tại e
* vậy G_xe và G_ye là nghiệm của hệ phương trình 2 ẩn của 2 đường thẳng trên:
*/
float D = A_he * leq.b - leq.a * B_he;
float Dx = (-C_he) * leq.b - (-leq.c) * B_he;
float Dy = A_he * (-leq.c) - leq.a * (-C_he);
/*
* Tính tọa độ điểm E
*/
if (D == 0)
{
if (Dx + Dy == 0)
{
/*
* Vì hai đường thẳng trùng nhau nên sai số giữa robot và đường đi là bằng 0
*/
present_pose.error_line = 0;
}
else
throw NoSolution();
}
else
{
float G_xe = Dx / D;
float G_ye = Dy / D;
/*
* Tính khoảng cách từ điểm E đến đường thẳng (RH)
*/
present_pose.error_line = -(A_rh * G_xe + B_rh * G_ye + C_rh) / (sqrt(pow(A_rh, 2) + pow(B_rh, 2)));
}
present_pose.distanceToEnd = sqrt(pow((leq.Gj.x - G_xr), 2) + pow((leq.Gj.y - G_yr), 2));
present_pose.distanceToStart = sqrt(pow((leq.Gi.x - G_xr), 2) + pow((leq.Gi.y - G_yr), 2));
return present_pose;
}
} /* namespace dwb_critics */

99
navigations/dwb_critics/src/obstacle_footprint.cpp Executable file
View File

@@ -0,0 +1,99 @@
/*
* 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.
*/
#include <dwb_critics/obstacle_footprint.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_grid_iterators/polygon_outline.h>
#include <nav_2d_utils/polygons.h>
#include <nav_2d_utils/footprint.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
#include <algorithm>
#include <vector>
PLUGINLIB_EXPORT_CLASS(dwb_critics::ObstacleFootprintCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
void ObstacleFootprintCritic::onInit()
{
BaseObstacleCritic::onInit();
footprint_spec_ = nav_2d_utils::footprintFromParams(critic_nh_);
}
bool ObstacleFootprintCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan)
{
if (footprint_spec_.points.size() == 0)
{
ROS_ERROR_NAMED("ObstacleFootprintCritic", "Footprint spec is empty, maybe missing call to setFootprint?");
return false;
}
return true;
}
double ObstacleFootprintCritic::scorePose(const nav_core2::Costmap& costmap, const geometry_msgs::Pose2D& pose)
{
unsigned int cell_x, cell_y;
if (!worldToGridBounded(costmap.getInfo(), pose.x, pose.y, cell_x, cell_y))
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
return scorePose(costmap, pose, nav_2d_utils::movePolygonToPose(footprint_spec_, pose));
}
double ObstacleFootprintCritic::scorePose(const nav_core2::Costmap& costmap, const geometry_msgs::Pose2D& pose,
const nav_2d_msgs::Polygon2D& footprint)
{
unsigned char footprint_cost = 0;
nav_grid::NavGridInfo info = costmap.getInfo();
for (nav_grid::Index index : nav_grid_iterators::PolygonOutline(&info, footprint))
{
unsigned char cost = costmap(index.x, index.y);
// if the cell is in an obstacle the path is invalid or unknown
if (cost == costmap.LETHAL_OBSTACLE)
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
}
else if (cost == costmap.NO_INFORMATION)
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Unknown Region.");
}
footprint_cost = std::max(cost, footprint_cost);
}
// if all line costs are legal... then we can return that the footprint is legal
return footprint_cost;
}
} // namespace dwb_critics

223
navigations/dwb_critics/src/oscillation.cpp Executable file
View File

@@ -0,0 +1,223 @@
/*
* 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.
*/
#include <nav_2d_utils/parameters.h>
#include <dwb_critics/oscillation.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
#include <cmath>
#include <string>
#include <vector>
PLUGINLIB_EXPORT_CLASS(dwb_critics::OscillationCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
OscillationCritic::CommandTrend::CommandTrend()
{
reset();
}
void OscillationCritic::CommandTrend::reset()
{
sign_ = Sign::ZERO;
positive_only_ = false;
negative_only_ = false;
}
bool OscillationCritic::CommandTrend::update(double velocity)
{
bool flag_set = false;
if (velocity < 0.0)
{
if (sign_ == Sign::POSITIVE)
{
negative_only_ = true;
flag_set = true;
}
sign_ = Sign::NEGATIVE;
}
else if (velocity > 0.0)
{
if (sign_ == Sign::NEGATIVE)
{
positive_only_ = true;
flag_set = true;
}
sign_ = Sign::POSITIVE;
}
return flag_set;
}
bool OscillationCritic::CommandTrend::isOscillating(double velocity)
{
return (positive_only_ && velocity < 0.0) || (negative_only_ && velocity > 0.0);
}
bool OscillationCritic::CommandTrend::hasSignFlipped()
{
return positive_only_ || negative_only_;
}
void OscillationCritic::onInit()
{
oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam(critic_nh_, "oscillation_reset_dist", 0.05);
oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_;
oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam(critic_nh_, "oscillation_reset_angle", 0.2);
oscillation_reset_time_ = nav_2d_utils::searchAndGetParam(critic_nh_, "oscillation_reset_time", -1.0);
/**
* Historical Parameter Loading
* If x_only_threshold is set, use that.
* If min_speed_xy is set in the namespace (as it is often used for trajectory generation), use that.
* If min_trans_vel is set in the namespace, as it used to be used for trajectory generation, complain then use that.
* Otherwise, set x_only_threshold_ to 0.05
*/
std::string resolved_name;
if (critic_nh_.hasParam("x_only_threshold"))
{
critic_nh_.getParam("x_only_threshold", x_only_threshold_);
}
else if (critic_nh_.searchParam("min_speed_xy", resolved_name))
{
critic_nh_.getParam(resolved_name, x_only_threshold_);
}
else if (critic_nh_.searchParam("min_trans_vel", resolved_name))
{
ROS_WARN_NAMED("OscillationCritic", "Parameter min_trans_vel is deprecated. "
"Please use the name min_speed_xy or x_only_threshold instead.");
critic_nh_.getParam(resolved_name, x_only_threshold_);
}
else
{
x_only_threshold_ = 0.05;
}
reset();
}
bool OscillationCritic::prepare(const geometry_msgs::Pose2D& pose,
const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan)
{
pose_ = pose;
return true;
}
void OscillationCritic::debrief(const nav_2d_msgs::Twist2D& cmd_vel)
{
if (setOscillationFlags(cmd_vel))
{
prev_stationary_pose_ = pose_;
prev_reset_time_ = ros::Time::now();
}
// if we've got restrictions... check if we can reset any oscillation flags
if (x_trend_.hasSignFlipped() || y_trend_.hasSignFlipped() || theta_trend_.hasSignFlipped())
{
// Reset flags if enough time or distance has passed
if (resetAvailable())
{
reset();
}
}
}
bool OscillationCritic::resetAvailable()
{
if (oscillation_reset_dist_ >= 0.0)
{
double x_diff = pose_.x - prev_stationary_pose_.x;
double y_diff = pose_.y - prev_stationary_pose_.y;
double sq_dist = x_diff * x_diff + y_diff * y_diff;
if (sq_dist > oscillation_reset_dist_sq_)
{
return true;
}
}
if (oscillation_reset_angle_ >= 0.0)
{
double th_diff = pose_.theta - prev_stationary_pose_.theta;
if (fabs(th_diff) > oscillation_reset_angle_)
{
return true;
}
}
if (oscillation_reset_time_ >= 0.0)
{
double t_diff = (ros::Time::now() - prev_reset_time_).toSec();
if (t_diff > oscillation_reset_time_)
{
return true;
}
}
return false;
}
void OscillationCritic::reset()
{
x_trend_.reset();
y_trend_.reset();
theta_trend_.reset();
}
bool OscillationCritic::setOscillationFlags(const nav_2d_msgs::Twist2D& cmd_vel)
{
bool flag_set = false;
// set oscillation flags for moving forward and backward
flag_set |= x_trend_.update(cmd_vel.x);
// we'll only set flags for strafing and rotating when we're not moving forward at all
if (x_only_threshold_ < 0.0 || fabs(cmd_vel.x) <= x_only_threshold_)
{
flag_set |= y_trend_.update(cmd_vel.y);
flag_set |= theta_trend_.update(cmd_vel.theta);
}
return flag_set;
}
double OscillationCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
{
if (x_trend_.isOscillating(traj.velocity.x) ||
y_trend_.isOscillating(traj.velocity.y) ||
theta_trend_.isOscillating(traj.velocity.theta))
{
throw nav_core2::IllegalTrajectoryException(name_, "Trajectory is oscillating.");
}
return 0.0;
}
} // namespace dwb_critics

87
navigations/dwb_critics/src/path_align.cpp Executable file
View File

@@ -0,0 +1,87 @@
/*
* 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.
*/
#include <dwb_critics/path_align.h>
#include <dwb_critics/alignment_util.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/parameters.h>
#include <vector>
#include <string>
namespace dwb_critics
{
void PathAlignCritic::onInit()
{
PathDistCritic::onInit();
stop_on_failure_ = false;
forward_point_distance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "forward_point_distance", 0.325);
}
bool PathAlignCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan)
{
double dx = pose.x - goal.x;
double dy = pose.y - goal.y;
double sq_dist = dx * dx + dy * dy;
if (sq_dist > forward_point_distance_ * forward_point_distance_)
{
zero_scale_ = false;
}
else
{
// once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
zero_scale_ = true;
return true;
}
return PathDistCritic::prepare(pose, vel, goal, global_plan);
}
double PathAlignCritic::getScale() const
{
if (zero_scale_)
return 0.0;
else
return costmap_->getResolution() * 0.5 * scale_;
}
double PathAlignCritic::scorePose(const geometry_msgs::Pose2D& pose)
{
return PathDistCritic::scorePose(getForwardPose(pose, forward_point_distance_));
}
} // namespace dwb_critics
PLUGINLIB_EXPORT_CLASS(dwb_critics::PathAlignCritic, dwb_local_planner::TrajectoryCritic)

92
navigations/dwb_critics/src/path_dist.cpp Executable file
View File

@@ -0,0 +1,92 @@
/*
* 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.
*/
#include <dwb_critics/path_dist.h>
#include <nav_grid/coordinate_conversion.h>
#include <pluginlib/class_list_macros.h>
#include <nav_2d_utils/path_ops.h>
#include <vector>
namespace dwb_critics
{
bool PathDistCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan)
{
reset();
const nav_core2::Costmap& costmap = *costmap_;
const nav_grid::NavGridInfo& info = costmap.getInfo();
bool started_path = false;
nav_2d_msgs::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution);
if (adjusted_global_plan.poses.size() != global_plan.poses.size())
{
ROS_DEBUG_NAMED("PathDistCritic", "Adjusted global plan resolution, added %zu points",
adjusted_global_plan.poses.size() - global_plan.poses.size());
}
unsigned int i;
// put global path points into local map until we reach the border of the local map
for (i = 0; i < adjusted_global_plan.poses.size(); ++i)
{
double g_x = adjusted_global_plan.poses[i].x;
double g_y = adjusted_global_plan.poses[i].y;
unsigned int map_x, map_y;
if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != costmap.NO_INFORMATION)
{
cell_values_.setValue(map_x, map_y, 0.0);
queue_->enqueueCell(map_x, map_y);
started_path = true;
}
else if (started_path)
{
break;
}
}
if (!started_path)
{
ROS_ERROR_NAMED("PathDistCritic",
"None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.poses.size(), global_plan.poses.size());
return false;
}
propogateManhattanDistances();
return true;
}
} // namespace dwb_critics
PLUGINLIB_EXPORT_CLASS(dwb_critics::PathDistCritic, dwb_local_planner::TrajectoryCritic)

69
navigations/dwb_critics/src/prefer_forward.cpp Executable file
View File

@@ -0,0 +1,69 @@
/*
* 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.
*/
#include <dwb_critics/prefer_forward.h>
#include <math.h>
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(dwb_critics::PreferForwardCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
void PreferForwardCritic::onInit()
{
critic_nh_.param("penalty", penalty_, 1.0);
critic_nh_.param("strafe_x", strafe_x_, 0.1);
critic_nh_.param("strafe_theta", strafe_theta_, 0.2);
critic_nh_.param("theta_scale", theta_scale_, 10.0);
}
double PreferForwardCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
{
// backward motions bad on a robot without backward sensors
if (traj.velocity.x < 0.0)
{
return penalty_;
}
// strafing motions also bad on such a robot
if (traj.velocity.x < strafe_x_ && fabs(traj.velocity.theta) < strafe_theta_)
{
return penalty_;
}
// the more we rotate, the less we progress forward
return fabs(traj.velocity.theta) * theta_scale_;
}
} /* namespace dwb_critics */

130
navigations/dwb_critics/src/rotate_to_goal.cpp Executable file
View File

@@ -0,0 +1,130 @@
/*
* 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.
*/
#include <dwb_critics/rotate_to_goal.h>
#include <dwb_local_planner/trajectory_utils.h>
#include <nav_2d_utils/parameters.h>
#include <nav_core2/exceptions.h>
#include <pluginlib/class_list_macros.h>
#include <angles/angles.h>
#include <string>
#include <vector>
const double EPSILON = 1E-5;
PLUGINLIB_EXPORT_CLASS(dwb_critics::RotateToGoalCritic, dwb_local_planner::TrajectoryCritic)
namespace dwb_critics
{
inline double hypot_sq(double dx, double dy)
{
return dx * dx + dy * dy;
}
void RotateToGoalCritic::onInit()
{
xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "xy_goal_tolerance", 0.25);
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
double stopped_xy_velocity = nav_2d_utils::searchAndGetParam(critic_nh_, "trans_stopped_velocity", 0.25);
stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity;
critic_nh_.param("slowing_factor", slowing_factor_, 5.0);
critic_nh_.param("lookahead_time", lookahead_time_, -1.0);
reset();
}
void RotateToGoalCritic::reset()
{
in_window_ = false;
rotating_ = false;
}
bool RotateToGoalCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
const geometry_msgs::Pose2D& goal,
const nav_2d_msgs::Path2D& global_plan)
{
double dxy_sq = hypot_sq(pose.x - goal.x, pose.y - goal.y);
in_window_ = in_window_ || dxy_sq <= xy_goal_tolerance_sq_;
current_xy_speed_sq_ = hypot_sq(vel.x, vel.y);
rotating_ = rotating_ || (in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_);
goal_yaw_ = goal.theta;
return true;
}
double RotateToGoalCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
{
// If we're not sufficiently close to the goal, we don't care what the twist is
if (!in_window_)
{
return 0.0;
}
else if (!rotating_)
{
double speed_sq = hypot_sq(traj.velocity.x, traj.velocity.y);
if (speed_sq >= current_xy_speed_sq_)
{
throw nav_core2::IllegalTrajectoryException(name_, "Not slowing down near goal.");
}
return speed_sq * slowing_factor_ + scoreRotation(traj);
}
// If we're sufficiently close to the goal, any transforming velocity is invalid
if (fabs(traj.velocity.x) > EPSILON || fabs(traj.velocity.y) > EPSILON)
{
throw nav_core2::IllegalTrajectoryException(name_, "Nonrotation command near goal.");
}
return scoreRotation(traj);
}
double RotateToGoalCritic::scoreRotation(const dwb_msgs::Trajectory2D& traj)
{
if (traj.poses.empty())
{
throw nav_core2::IllegalTrajectoryException(name_, "Empty trajectory.");
}
double end_yaw;
if (lookahead_time_ >= 0.0)
{
geometry_msgs::Pose2D eval_pose = dwb_local_planner::projectPose(traj, lookahead_time_);
end_yaw = eval_pose.theta;
}
else
{
end_yaw = traj.poses.back().theta;
}
return fabs(angles::shortest_angular_distance(end_yaw, goal_yaw_));
}
} /* namespace dwb_critics */

55
navigations/dwb_critics/src/twirling.cpp Executable file
View File

@@ -0,0 +1,55 @@
/*
* 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.
*/
#include <dwb_critics/twirling.h>
#include <pluginlib/class_list_macros.h>
namespace dwb_critics
{
void TwirlingCritic::onInit()
{
// Scale is set to 0 by default, so if it was not set otherwise, set to 0
if (!critic_nh_.hasParam("scale"))
{
scale_ = 0.0;
}
}
double TwirlingCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
{
return fabs(traj.velocity.theta); // add cost for making the robot spin
}
} // namespace dwb_critics
PLUGINLIB_EXPORT_CLASS(dwb_critics::TwirlingCritic, dwb_local_planner::TrajectoryCritic)