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

55
navigations/dwb_local_planner/include/dwb_local_planner/backwards_compatibility.h 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.
*/
#ifndef DWB_LOCAL_PLANNER_BACKWARDS_COMPATIBILITY_H
#define DWB_LOCAL_PLANNER_BACKWARDS_COMPATIBILITY_H
#include <ros/ros.h>
#include <string>
namespace dwb_local_planner
{
std::string getBackwardsCompatibleDefaultGenerator(const ros::NodeHandle& nh);
/**
* @brief Load parameters to emulate dwa_local_planner
*
* If no critic parameters are specified, this function should be called
* to load/move parameters that will emulate the behavior of dwa_local_planner
*
* @param nh NodeHandle to load parameters from
*/
void loadBackwardsCompatibleParameters(const ros::NodeHandle& nh);
} // namespace dwb_local_planner
#endif // DWB_LOCAL_PLANNER_BACKWARDS_COMPATIBILITY_H

81
navigations/dwb_local_planner/include/dwb_local_planner/debug_dwb_local_planner.h Executable file
View File

@@ -0,0 +1,81 @@
/*
* 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_LOCAL_PLANNER_DEBUG_DWB_LOCAL_PLANNER_H
#define DWB_LOCAL_PLANNER_DEBUG_DWB_LOCAL_PLANNER_H
#include <dwb_local_planner/dwb_local_planner.h>
#include <dwb_msgs/DebugLocalPlan.h>
#include <dwb_msgs/GenerateTwists.h>
#include <dwb_msgs/GenerateTrajectory.h>
#include <dwb_msgs/ScoreTrajectory.h>
#include <dwb_msgs/GetCriticScore.h>
#include <string>
namespace dwb_local_planner
{
/**
* @brief A version of DWBLocalPlanner with ROS services for the major components.
*
* Advertises three services: GenerateTwists, GenerateTrajectory and DebugLocalPlan
*/
class DebugDWBLocalPlanner: public DWBLocalPlanner
{
public:
/**
* @brief Override the DWB constructor to also advertise the services
*/
void initialize(const ros::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
protected:
bool generateTwistsService(dwb_msgs::GenerateTwists::Request &req,
dwb_msgs::GenerateTwists::Response &res);
bool generateTrajectoryService(dwb_msgs::GenerateTrajectory::Request &req,
dwb_msgs::GenerateTrajectory::Response &res);
bool scoreTrajectoryService(dwb_msgs::ScoreTrajectory::Request &req,
dwb_msgs::ScoreTrajectory::Response &res);
bool getCriticScoreService(dwb_msgs::GetCriticScore::Request &req,
dwb_msgs::GetCriticScore::Response &res);
bool debugLocalPlanService(dwb_msgs::DebugLocalPlan::Request &req,
dwb_msgs::DebugLocalPlan::Response &res);
TrajectoryCritic::Ptr getCritic(std::string name);
ros::ServiceServer twist_gen_service_, generate_traj_service_, score_service_, critic_service_, debug_service_;
};
} // namespace dwb_local_planner
#endif // DWB_LOCAL_PLANNER_DEBUG_DWB_LOCAL_PLANNER_H

217
navigations/dwb_local_planner/include/dwb_local_planner/dwb_local_planner.h Executable file
View File

@@ -0,0 +1,217 @@
/*
* 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_LOCAL_PLANNER_DWB_LOCAL_PLANNER_H
#define DWB_LOCAL_PLANNER_DWB_LOCAL_PLANNER_H
#include <dwb_local_planner/trajectory_generator.h>
#include <dwb_local_planner/goal_checker.h>
#include <dwb_local_planner/trajectory_critic.h>
#include <dwb_local_planner/publisher.h>
#include <nav_core2/local_planner.h>
#include <pluginlib/class_loader.h>
#include <memory>
#include <string>
#include <vector>
namespace dwb_local_planner
{
/**
* @class DWBLocalPlanner
* @brief Plugin-based flexible local_planner
*/
class DWBLocalPlanner: public nav_core2::LocalPlanner
{
public:
/**
* @brief Constructor that brings up pluginlib loaders
*/
DWBLocalPlanner();
virtual ~DWBLocalPlanner() {}
/**
* @brief nav_core2 initialization
* @param name Namespace for this planner
* @param tf TFListener pointer
* @param costmap_ros Costmap pointer
*/
void initialize(const ros::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
/**
* @brief nav_core2 setGoalPose - Sets the global goal pose
* @param goal_pose The Goal Pose
*/
void setGoalPose(const nav_2d_msgs::Pose2DStamped& goal_pose) override;
/**
* @brief nav_core2 setPlan - Sets the global plan
* @param path The global plan
*/
void setPlan(const nav_2d_msgs::Path2D& path) override;
/**
* @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
*
* It is presumed that the global plan is already set.
*
* This is mostly a wrapper for the protected computeVelocityCommands
* function which has additional debugging info.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @return The best command for the robot to drive
*/
nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
const nav_2d_msgs::Twist2D& velocity) override;
/**
* @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
*
* The pose that it checks against is the last pose in the current global plan.
* The calculation is delegated to the goal_checker plugin.
*
* @param pose Current pose
* @param velocity Current velocity
* @return True if the robot should be considered as having reached the goal.
*/
bool isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) override;
/**
* @brief Score a given command. Can be used for testing.
*
* Given a trajectory, calculate the score where lower scores are better.
* If the given (positive) score exceeds the best_score, calculation may be cut short, as the
* score can only go up from there.
*
* @param traj Trajectory to check
* @param best_score If positive, the threshold for early termination
* @return The full scoring of the input trajectory
*/
virtual dwb_msgs::TrajectoryScore scoreTrajectory(const dwb_msgs::Trajectory2D& traj, double best_score = -1);
/**
* @brief Compute the best command given the current pose and velocity, with possible debug information
*
* Same as above computeVelocityCommands, but with debug results.
* If the results pointer is not null, additional information about the twists
* evaluated will be in results after the call.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @param results Output param, if not null, will be filled in with full evaluation results
* @return Best command
*/
virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
const nav_2d_msgs::Twist2D& velocity,
std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
protected:
/**
* @brief Helper method for preparing for the core scoring algorithm
*
* Runs the prepare method on all the critics with freshly transformed data
*/
virtual void prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity);
/**
* @brief Iterate through all the twists and find the best one
*/
virtual dwb_msgs::TrajectoryScore coreScoringAlgorithm(const geometry_msgs::Pose2D& pose,
const nav_2d_msgs::Twist2D velocity,
std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
/**
* @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses
*
* Three key operations
* 1) Transforms global plan into frame of the given pose
* 2) Only returns poses that are near the robot, i.e. whether they are likely on the local costmap
* 3) If prune_plan_ is true, it will remove all points that we've already passed from both the transformed plan
* and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_
* of the robot and erases all poses before that.
*/
virtual nav_2d_msgs::Path2D transformGlobalPlan(const nav_2d_msgs::Pose2DStamped& pose);
/**
* @brief Helper method to transform a given pose to the local costmap frame.
*/
geometry_msgs::Pose2D transformPoseToLocal(const nav_2d_msgs::Pose2DStamped& pose);
nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan
nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
bool prune_plan_;
double prune_distance_;
bool debug_trajectory_details_;
bool short_circuit_trajectory_evaluation_;
// Plugin handling
pluginlib::ClassLoader<TrajectoryGenerator> traj_gen_loader_;
TrajectoryGenerator::Ptr traj_generator_;
pluginlib::ClassLoader<GoalChecker> goal_checker_loader_;
GoalChecker::Ptr goal_checker_;
pluginlib::ClassLoader<TrajectoryCritic> critic_loader_;
std::vector<TrajectoryCritic::Ptr> critics_;
/**
* @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic"
*
* @param base_name The name of the critic as read in from the parameter server
* @return Our attempted resolution of the name, with namespace prepended and/or the suffix Critic appended
*/
std::string resolveCriticClassName(std::string base_name);
/**
* @brief Load the critic parameters from the namespace
* @param name The namespace of this planner.
*/
virtual void loadCritics(const std::string name);
std::vector<std::string> default_critic_namespaces_;
nav_core2::Costmap::Ptr costmap_;
bool update_costmap_before_planning_;
TFListenerPtr tf_;
DWBPublisher pub_;
ros::NodeHandle planner_nh_;
ros::Publisher traj_pub_;
};
} // namespace dwb_local_planner
#endif // DWB_LOCAL_PLANNER_DWB_LOCAL_PLANNER_H

86
navigations/dwb_local_planner/include/dwb_local_planner/goal_checker.h Executable file
View File

@@ -0,0 +1,86 @@
/*
* 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_LOCAL_PLANNER_GOAL_CHECKER_H
#define DWB_LOCAL_PLANNER_GOAL_CHECKER_H
#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <nav_2d_msgs/Twist2D.h>
#include <memory>
namespace dwb_local_planner
{
/**
* @class GoalChecker
* @brief Function-object for checking whether a goal has been reached
*
* This class defines the plugin interface for determining whether you have reached
* the goal state. This primarily consists of checking the relative positions of two poses
* (which are presumed to be in the same frame). It can also check the velocity, as some
* applications require that robot be stopped to be considered as having reached the goal.
*/
class GoalChecker
{
public:
using Ptr = std::shared_ptr<dwb_local_planner::GoalChecker>;
virtual ~GoalChecker() {}
/**
* @brief Initialize any parameters from the NodeHandle
* @param nh NodeHandle for grabbing parameters
*/
virtual void initialize(const ros::NodeHandle& nh) = 0;
/**
* @brief Reset the state of the goal checker (if any)
*/
virtual void reset() {}
/**
* @brief Check whether the goal should be considered reached
* @param query_pose The pose to check
* @param goal_pose The pose to check against
* @param velocity The robot's current velocity
* @return True if goal is reached
*/
virtual bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose,
const nav_2d_msgs::Twist2D& velocity) = 0;
};
} // namespace dwb_local_planner
#endif // DWB_LOCAL_PLANNER_GOAL_CHECKER_H

75
navigations/dwb_local_planner/include/dwb_local_planner/illegal_trajectory_tracker.h Executable file
View File

@@ -0,0 +1,75 @@
/*
* 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_LOCAL_PLANNER_ILLEGAL_TRAJECTORY_TRACKER_H
#define DWB_LOCAL_PLANNER_ILLEGAL_TRAJECTORY_TRACKER_H
#include <nav_core2/exceptions.h>
#include <map>
#include <utility>
#include <string>
namespace dwb_local_planner
{
class IllegalTrajectoryTracker
{
public:
IllegalTrajectoryTracker() : legal_count_(0), illegal_count_(0) {}
void addIllegalTrajectory(const nav_core2::IllegalTrajectoryException& e);
void addLegalTrajectory();
std::map< std::pair<std::string, std::string>, double> getPercentages() const;
std::string getMessage() const;
protected:
std::map< std::pair<std::string, std::string>, unsigned int> counts_;
unsigned int legal_count_, illegal_count_;
};
/**
* @class NoLegalTrajectoriesException
* @brief Thrown when all the trajectories explored are illegal
*/
class NoLegalTrajectoriesException: public nav_core2::NoLegalTrajectoriesException
{
public:
explicit NoLegalTrajectoriesException(const IllegalTrajectoryTracker& tracker)
: nav_core2::NoLegalTrajectoriesException(tracker.getMessage()), tracker_(tracker) {}
IllegalTrajectoryTracker tracker_;
};
} // namespace dwb_local_planner
#endif // DWB_LOCAL_PLANNER_ILLEGAL_TRAJECTORY_TRACKER_H

107
navigations/dwb_local_planner/include/dwb_local_planner/publisher.h Executable file
View File

@@ -0,0 +1,107 @@
/*
* 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_LOCAL_PLANNER_PUBLISHER_H
#define DWB_LOCAL_PLANNER_PUBLISHER_H
#include <ros/ros.h>
#include <nav_core2/common.h>
#include <dwb_local_planner/trajectory_critic.h>
#include <dwb_msgs/LocalPlanEvaluation.h>
#include <memory>
#include <vector>
namespace dwb_local_planner
{
/**
* @class DWBPublisher
* @brief Consolidation of all the publishing logic for the DWB Local Planner.
*
* Right now, it can publish
* 1) The Global Plan (as passed in using setPath)
* 2) The Local Plan (after it is calculated)
* 3) The Transformed Global Plan (since it may be different than the global)
* 4) The Full LocalPlanEvaluation
* 5) Markers representing the different trajectories evaluated
* 6) The CostGrid (in the form of a complex PointCloud2)
*/
class DWBPublisher
{
public:
/**
* @brief Load the parameters and advertise topics as needed
* @param nh NodeHandle to load parameters from
*/
void initialize(ros::NodeHandle& nh);
/**
* @brief Does the publisher require that the LocalPlanEvaluation be saved
* @return True if the Evaluation is needed to publish either directly or as trajectories
*/
bool shouldRecordEvaluation() { return publish_evaluation_ || publish_trajectories_; }
/**
* @brief If the pointer is not null, publish the evaluation and trajectories as needed
*/
void publishEvaluation(std::shared_ptr<dwb_msgs::LocalPlanEvaluation> results);
void publishLocalPlan(const std_msgs::Header& header, const dwb_msgs::Trajectory2D& traj);
void publishCostGrid(const nav_core2::Costmap::Ptr costmap, const std::vector<TrajectoryCritic::Ptr> critics);
void publishGlobalPlan(const nav_2d_msgs::Path2D plan);
void publishTransformedPlan(const nav_2d_msgs::Path2D plan);
void publishLocalPlan(const nav_2d_msgs::Path2D plan);
void publishInputParams(const nav_grid::NavGridInfo& info, const geometry_msgs::Pose2D& start_pose,
const nav_2d_msgs::Twist2D& velocity, const geometry_msgs::Pose2D& goal_pose);
protected:
void publishTrajectories(const dwb_msgs::LocalPlanEvaluation& results);
// Helper function for publishing other plans
void publishGenericPlan(const nav_2d_msgs::Path2D plan, const ros::Publisher pub, bool flag);
// Flags for turning on/off publishing specific components
bool publish_evaluation_, publish_global_plan_, publish_transformed_, publish_local_plan_, publish_trajectories_;
bool publish_cost_grid_pc_, publish_input_params_;
// Marker Lifetime
ros::Duration marker_lifetime_;
// Publisher Objects
ros::Publisher eval_pub_, global_pub_, transformed_pub_, local_pub_, marker_pub_, cost_grid_pc_pub_,
info_pub_, pose_pub_, goal_pub_, velocity_pub_;
};
} // namespace dwb_local_planner
#endif // DWB_LOCAL_PLANNER_PUBLISHER_H

177
navigations/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h Executable file
View File

@@ -0,0 +1,177 @@
/*
* 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_LOCAL_PLANNER_TRAJECTORY_CRITIC_H
#define DWB_LOCAL_PLANNER_TRAJECTORY_CRITIC_H
#include <ros/ros.h>
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <geometry_msgs/Pose2D.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Path2D.h>
#include <dwb_msgs/Trajectory2D.h>
#include <sensor_msgs/PointCloud.h>
#include <memory>
#include <string>
#include <vector>
namespace dwb_local_planner
{
/**
* @class TrajectoryCritic
* @brief Evaluates a Trajectory2D to produce a score
*
* This class defines the plugin interface for the TrajectoryCritic which
* gives scores to trajectories, where lower numbers are better, but negative
* scores are considered invalid.
*
* The general lifecycle is
* 1) initialize is called once at the beginning which in turn calls onInit.
* Derived classes may override onInit to load parameters as needed.
* 2) prepare is called once before each set of trajectories.
* It is presumed that there are multiple trajectories that we want to evaluate,
* and there may be some shared work that can be done beforehand to optimize
* the scoring of each individual trajectory.
* 3) scoreTrajectory is called once per trajectory and returns the score.
* 4) debrief is called after each set of trajectories with the chosen trajectory.
* This can be used for stateful critics that monitor the trajectory through time.
*
* Optionally, there is also a debugging mechanism for certain types of critics in the
* addCriticVisualization method. If the score for a trajectory depends on its relationship to
* the costmap, addCriticVisualization can provide that information to the dwb_local_planner
* which will publish the grid scores as a PointCloud2.
*/
class TrajectoryCritic
{
public:
using Ptr = std::shared_ptr<dwb_local_planner::TrajectoryCritic>;
virtual ~TrajectoryCritic() {}
/**
* @brief Initialize the critic with appropriate pointers and parameters
*
* The name and costmap are stored as member variables.
* A NodeHandle for the critic is created with the namespace of the planner NodeHandle
*
* @param planner_nh Planner Nodehandle
* @param name The name of this critic
* @param costmap_ros Pointer to the costmap
*/
void initialize(const ros::NodeHandle& planner_nh, std::string name, nav_core2::Costmap::Ptr costmap)
{
name_ = name;
costmap_ = costmap;
planner_nh_ = planner_nh;
critic_nh_ = ros::NodeHandle(planner_nh_, name_);
critic_nh_.param("scale", scale_, 1.0);
onInit();
}
virtual void onInit() {}
/**
* @brief Reset the state of the critic
*
* Reset is called when the planner receives a new global goal.
* This can be used to discard information specific to one plan.
*/
virtual void reset() {}
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param vel Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual 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)
{
return true;
}
/**
* @brief Return a raw score for the given trajectory.
*
* scores < 0 are considered invalid/errors, such as collisions
* This is the raw score in that the scale should not be applied to it.
*/
virtual double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) = 0;
/**
* @brief debrief informs the critic what the chosen cmd_vel was (if it cares)
*/
virtual void debrief(const nav_2d_msgs::Twist2D& cmd_vel) {}
/**
* @brief Add information to the given pointcloud for debugging costmap-grid based scores
*
* addCriticVisualization is an optional debugging mechanism for providing rich information
* about the cost for certain trajectories. Some critics will have scoring mechanisms
* wherein there will be some score for each cell in the costmap. This could be as
* straightforward as the cost in the costmap, or it could be the number of cells away
* from the goal pose.
*
* Prior to calling this, dwb_local_planner will load the PointCloud's header and the points
* in row-major order. The critic may then add a ChannelFloat to the channels member of the PC
* with the same number of values as the points array. This information may then be converted
* and published as a PointCloud2.
*
* @param pc PointCloud to add channels to
*/
virtual void addCriticVisualization(sensor_msgs::PointCloud& pc) {}
std::string getName()
{
return name_;
}
virtual double getScale() const { return scale_; }
void setScale(const double scale) { scale_ = scale; }
protected:
std::string name_;
nav_core2::Costmap::Ptr costmap_;
double scale_;
ros::NodeHandle critic_nh_, planner_nh_;
};
} // namespace dwb_local_planner
#endif // DWB_LOCAL_PLANNER_TRAJECTORY_CRITIC_H

130
navigations/dwb_local_planner/include/dwb_local_planner/trajectory_generator.h 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.
*/
#ifndef DWB_LOCAL_PLANNER_TRAJECTORY_GENERATOR_H
#define DWB_LOCAL_PLANNER_TRAJECTORY_GENERATOR_H
#include <ros/ros.h>
#include <nav_2d_msgs/Twist2D.h>
#include <dwb_msgs/Trajectory2D.h>
#include <memory>
#include <vector>
namespace dwb_local_planner
{
/**
* @class TrajectoryGenerator
* @brief Interface for iterating through possible velocities and creating trajectories
*
* This class defines the plugin interface for two separate but related components.
*
* First, this class provides an iterator interface for exploring all of the velocities
* to search, given the current velocity.
*
* Second, the class gives an independent interface for creating a trajectory from a twist,
* i.e. projecting it out in time and space.
*
* Both components rely heavily on the robot's kinematic model, and can share many parameters,
* which is why they are grouped into a singular class.
*/
class TrajectoryGenerator
{
public:
using Ptr = std::shared_ptr<dwb_local_planner::TrajectoryGenerator>;
virtual ~TrajectoryGenerator() {}
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(ros::NodeHandle& nh) = 0;
/**
* @brief Reset the state (if any) when the planner gets a new goal
*/
virtual void reset() {}
/**
* @brief Start a new iteration based on the current velocity
* @param current_velocity
*/
virtual void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity) = 0;
/**
* @brief Test to see whether there are more twists to test
* @return True if more twists, false otherwise
*/
virtual bool hasMoreTwists() = 0;
/**
* @brief Return the next twist and advance the iteration
* @return The Twist!
*/
virtual nav_2d_msgs::Twist2D nextTwist() = 0;
/**
* @brief Get all the twists for an iteration.
*
* Note: Resets the iterator if one is in process
*
* @param current_velocity
* @return all the twists
*/
virtual std::vector<nav_2d_msgs::Twist2D> getTwists(const nav_2d_msgs::Twist2D& current_velocity)
{
std::vector<nav_2d_msgs::Twist2D> twists;
startNewIteration(current_velocity);
while (hasMoreTwists())
{
twists.push_back(nextTwist());
}
return twists;
}
/**
* @brief Given a cmd_vel in the robot's frame and initial conditions, generate a Trajectory2D
* @param start_pose Current robot location
* @param start_vel Current robot velocity
* @param cmd_vel The desired command velocity
*/
virtual dwb_msgs::Trajectory2D generateTrajectory(const geometry_msgs::Pose2D& start_pose,
const nav_2d_msgs::Twist2D& start_vel,
const nav_2d_msgs::Twist2D& cmd_vel) = 0;
};
} // namespace dwb_local_planner
#endif // DWB_LOCAL_PLANNER_TRAJECTORY_GENERATOR_H

65
navigations/dwb_local_planner/include/dwb_local_planner/trajectory_utils.h Executable file
View File

@@ -0,0 +1,65 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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_LOCAL_PLANNER_TRAJECTORY_UTILS_H
#define DWB_LOCAL_PLANNER_TRAJECTORY_UTILS_H
#include <dwb_msgs/Trajectory2D.h>
namespace dwb_local_planner
{
/**
* @brief Helper function to find a pose in the trajectory with a particular time time_offset
* @param trajectory The trajectory to search
* @param time_offset The desired time_offset
* @return reference to the pose that is closest to the particular time offset
*
* Linearly searches through the poses. Once the poses time_offset is greater than the desired time_offset,
* the search ends, since the poses have increasing time_offsets.
*/
const geometry_msgs::Pose2D& getClosestPose(const dwb_msgs::Trajectory2D& trajectory, const double time_offset);
/**
* @brief Helper function to create a pose with an exact time_offset by linearly interpolating between existing poses
* @param trajectory The trajectory with pose and time offset information
* @param time_offset The desired time_offset
* @return New Pose2D with interpolated values
* @note If the given time offset is outside the bounds of the trajectory, the return pose will be either the first or last pose.
*/
geometry_msgs::Pose2D projectPose(const dwb_msgs::Trajectory2D& trajectory, const double time_offset);
} // namespace dwb_local_planner
#endif // DWB_LOCAL_PLANNER_TRAJECTORY_UTILS_H