diff --git a/CMakeLists.txt b/CMakeLists.txt
index f12c25078f5a0d12f815e031cb82fd225e70ea0b..d8eeffb30c659f8bc8eca14a2553f9db327cc817 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,6 +19,8 @@ find_package(Eigen3 REQUIRED)
add_definitions(${EIGEN_DEFINITIONS})
#find_package(iriutils REQUIRED)
+find_package(iriutils REQUIRED)
+find_package(autonomous_driving_tools REQUIRED)
# ********************************************************************
# Add topic, service and action definition here
@@ -80,9 +82,11 @@ include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
include_directories(${EIGEN3_INCLUDE_DIRS})
#include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${autonomous_driving_tools_INCLUDE_DIR})
## Declare a cpp library
-add_library(${PROJECT_NAME} src/ackermann_odom_helper.cpp src/ackermann_planner_util.cpp src/ackermann_planner_ros src/ackermann_planner.cpp src/ackermann_trajectory_generator.cpp src/heading_cost_function.cpp src/ackermann_trajectory_search.cpp)
+add_library(${PROJECT_NAME} src/ackermann_odom_helper.cpp src/ackermann_planner_util.cpp src/ackermann_planner_ros src/ackermann_planner.cpp src/ackermann_trajectory_generator.cpp src/heading_cost_function.cpp src/ackermann_trajectory_search.cpp src/ackermann_spline_generator.cpp)
## Declare a cpp executable
# add_executable(${PROJECT_NAME} <list of source files>)
@@ -92,6 +96,8 @@ add_library(${PROJECT_NAME} src/ackermann_odom_helper.cpp src/ackermann_planner_
# ********************************************************************
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
#target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${autonomous_driving_tools_LIBRARIES})
# ********************************************************************
# Add message headers dependencies
diff --git a/cfg/AckermannLocalPlanner.cfg b/cfg/AckermannLocalPlanner.cfg
index 97309a7c621f8b19014c84086f56dc87c04c7bbc..52e2a1c6d36e34a2752b9879bd11d5d427c6200e 100755
--- a/cfg/AckermannLocalPlanner.cfg
+++ b/cfg/AckermannLocalPlanner.cfg
@@ -40,68 +40,77 @@ from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
-# Name Type Reconfiguration level Description Default Min Max
-gen.add("max_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 10, 0, 20)
-gen.add("min_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10)
-gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0)
-gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0)
-
-gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 32.0, 0.0)
-gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 24.0, 0.0)
-gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0)
-
-gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0)
-gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0)
-gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0)
-
-gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325)
-
-gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0)
-gen.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0)
-
-gen.add("trans_vel_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1)
-gen.add("steer_angle_samples", int_t, 0, "The number of samples to use when exploring the steer angle space", 10, 1)
-gen.add("angular_vel_samples", int_t, 0, "The number of samples to use when exploring the angular velocity space", 10, 1)
-gen.add("use_steer_angle_cmd", bool_t, 0, "Whether to use the steer angle or the angular velocity.", False)
-gen.add("invert_steer_angle_cmd", bool_t, 0, "Whether to use the invert the steer angle command or not.", False)
-gen.add("use_stopped_steering", bool_t, 0, "Whether to allow steering while stopped or not.", True)
-gen.add("stopped_steering_angle_threshold", double_t, 0, "Angle difference threshold (rad) to perform steering change while stopped.", 0.05, 0)
-
-gen.add("use_stuck_check", bool_t, 0, "Whether to allow robot stuck check or not.", True)
-gen.add("stuck_check_vel_threshold", double_t, 0, "Velocity threshold (m/s) to consider robot stuck.", 0.05, 0)
-gen.add("stuck_check_max_occurrences", int_t, 0, "Max number of stuck occurrences allowed until moving to next segment or replaning.", 10, 0)
-
-gen.add("prune_plan", bool_t, 0, "Start following closest point of global plan, not first point (if different).", False)
-gen.add("xy_goal_tolerance", double_t, 0, "Within what maximum distance we consider the robot to be in goal", 0.1)
-gen.add("yaw_goal_tolerance", double_t, 0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1)
-gen.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1)
-gen.add("rot_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1)
-
-gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False)
-
-gen.add("max_trans_vel", double_t, 0, "maximum translational speed", 0.3, 0, 20)
-gen.add("min_trans_vel", double_t, 0, "minimum translational speed", -0.3, -20, 0)
-gen.add("max_trans_acc", double_t, 0, "maximum translational acceleration", 1.0, 0, 20)
-gen.add("max_steer_angle", double_t, 0, "maximum steer angle", 0.45, 0, 0.54)
-gen.add("min_steer_angle", double_t, 0, "minimum steer angle", -0.45, -0.54, 0)
-gen.add("max_steer_vel", double_t, 0, "maximum steer speed", 1.0, 0, 2)
-gen.add("min_steer_vel", double_t, 0, "minimum steer speed", -1.0, -2, 0)
-gen.add("max_steer_acc", double_t, 0, "maximum steer acceleration", 0.36, 0, 5)
-gen.add("max_angular_vel", double_t, 0, "maximum angular speed", 0.3, 0, 20)
-gen.add("min_angular_vel", double_t, 0, "minimum angular speed", -0.3, -20, 0)
-gen.add("axis_distance", double_t, 0, "distance between axes", 1.65, -2.0, 2)
-gen.add("wheel_distance", double_t, 0, "distance between wheels", 1.2, 0, 2)
-gen.add("wheel_radius", double_t, 0, "Wheel diameter", 0.436, 0, 2)
-gen.add("use_trans_vel_deadzone", bool_t, 0, "Whether to use a deadzone in the translational velocity or not", False)
-gen.add("trans_vel_deadzone", double_t, 0, "Translatinal velocity minimum value", 0.1, 0.01, 1.0)
-gen.add("split_ignore_length", double_t, 0, "Paths segments under this length will be ignored", 1.0, 0.0, 2.0)
-gen.add("cmd_vel_avg", int_t, 0, "Number of cmd_vel to average", 1, 1, 20)
-gen.add("odom_avg", int_t, 0, "Number of odom to average", 1, 1, 20)
-
-gen.add("planner_patience", int_t, 0, "number of impossible paths before replanning", 2, 0, 10)
-
-gen.add("hdiff_scale", double_t, 0, "The weight for the heading distance part of the cost function", 1.0, 0, 500)
-gen.add("heading_points", int_t, 0, "The number of points to check the heading", 8, 1, 64)
+trajectory = gen.add_group("trajectory")
+costs = gen.add_group("costs")
+planner = gen.add_group("planner")
+ackermann = gen.add_group("ackermann")
+splines = gen.add_group("splines")
+# Name Type Reconfiguration level Description Default Min Max
+planner.add("use_stopped_steering", bool_t, 0, "Whether to allow steering while stopped or not.", True)
+planner.add("stopped_steering_angle_threshold", double_t, 0, "Angle difference threshold (rad) to perform steering change while stopped.", 0.05, 0)
+planner.add("use_stuck_check", bool_t, 0, "Whether to allow robot stuck check or not.", True)
+planner.add("stuck_check_vel_threshold", double_t, 0, "Velocity threshold (m/s) to consider robot stuck.", 0.05, 0)
+planner.add("stuck_check_max_occurrences", int_t, 0, "Max number of stuck occurrences allowed until moving to next segment or replaning.", 10, 0)
+planner.add("prune_plan", bool_t, 0, "Start following closest point of global plan, not first point (if different).", False)
+planner.add("xy_goal_tolerance", double_t, 0, "Within what maximum distance we consider the robot to be in goal", 0.1)
+planner.add("yaw_goal_tolerance", double_t, 0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1)
+planner.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1)
+planner.add("rot_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1)
+planner.add("use_trans_vel_deadzone", bool_t, 0, "Whether to use a deadzone in the translational velocity or not", False)
+planner.add("trans_vel_deadzone", double_t, 0, "Translatinal velocity minimum value", 0.1, 0.01, 1.0)
+planner.add("cmd_vel_avg", int_t, 0, "Number of cmd_vel to average", 1, 1, 20)
+planner.add("odom_avg", int_t, 0, "Number of odom to average", 1, 1, 20)
+planner.add("planner_patience", int_t, 0, "number of impossible paths before replanning", 2, 0, 10)
+planner.add("check_path_r_inc", double_t, 0, "Radius increment robot's one when checking the path to get last safe point", 0.1, 0.0, 2.0)
+
+costs.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 32.0, 0.0)
+costs.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 24.0, 0.0)
+costs.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0)
+costs.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0)
+costs.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0)
+costs.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0)
+costs.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0)
+costs.add("hdiff_scale", double_t, 0, "The weight for the heading distance part of the cost function", 1.0, 0, 500)
+costs.add("heading_points", int_t, 0, "The number of points to check the heading", 8, 1, 64)
+
+trajectory.add("max_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 10, 0, 20)
+trajectory.add("min_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10)
+trajectory.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0)
+trajectory.add("max_trans_vel", double_t, 0, "maximum translational speed", 0.3, 0, 20)
+trajectory.add("min_trans_vel", double_t, 0, "minimum translational speed", -0.3, -20, 0)
+trajectory.add("max_trans_acc", double_t, 0, "maximum translational acceleration", 1.0, 0, 20)
+trajectory.add("max_steer_angle", double_t, 0, "maximum steer angle", 0.45, 0, 0.54)
+trajectory.add("min_steer_angle", double_t, 0, "minimum steer angle", -0.45, -0.54, 0)
+trajectory.add("max_steer_vel", double_t, 0, "maximum steer speed", 1.0, 0, 2)
+trajectory.add("min_steer_vel", double_t, 0, "minimum steer speed", -1.0, -2, 0)
+trajectory.add("max_steer_acc", double_t, 0, "maximum steer acceleration", 0.36, 0, 5)
+trajectory.add("max_angular_vel", double_t, 0, "maximum angular speed", 0.3, 0, 20)
+trajectory.add("min_angular_vel", double_t, 0, "minimum angular speed", -0.3, -20, 0)
+trajectory.add("max_curvature", double_t, 0, "Maximum allowed curvature for trajectory generation", 2.0, 0.5, 10.0)
+trajectory.add("trans_vel_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1)
+trajectory.add("steer_angle_samples", int_t, 0, "The number of samples to use when exploring the steer angle space", 10, 1)
+trajectory.add("angular_vel_samples", int_t, 0, "The number of samples to use when exploring the angular velocity space", 10, 1)
+
+ackermann.add("use_steer_angle_cmd", bool_t, 0, "Whether to use the steer angle or the angular velocity.", False)
+ackermann.add("invert_steer_angle_cmd", bool_t, 0, "Whether to use the invert the steer angle command or not.", False)
+ackermann.add("axis_distance", double_t, 0, "distance between axes", 1.65, -2.0, 2)
+ackermann.add("wheel_distance", double_t, 0, "distance between wheels", 1.2, 0, 2)
+ackermann.add("wheel_radius", double_t, 0, "Wheel diameter", 0.436, 0, 2)
+ackermann.add("split_ignore_length", double_t, 0, "Paths segments under this length will be ignored", 1.0, 0.0, 2.0)
+ackermann.add("comfort_lat_acc", double_t, 0, "Comfort lateral acceleration", 3.0, 0.0, 10.0)
+
+splines.add("use_splines", bool_t, 0, "Use splines to generate the trajectory candidates", False)
+splines.add("n1_samples", int_t, 0, "Number of samples for the N1 parameter of the spline", 10,1,100)
+splines.add("n2_samples", int_t, 0, "Number of samples for the N2 parameter of the spline", 10,1,100)
+splines.add("n3_samples", int_t, 0, "Number of samples for the N3 parameter of the spline", 1,1,100)
+splines.add("n4_samples", int_t, 0, "Number of samples for the N4 parameter of the spline", 1,1,100)
+splines.add("dist_samples", int_t, 0, "Number of distance samples for the current segment", 1,1,10)
+splines.add("min_segment_points", int_t, 0, "Minimum number of points of each segment", 10,1,100)
+splines.add("lateral_offset_gain", double_t, 0, "Gain for the lateral offset steering correction", 0.1,0.001,10)
+splines.add("temp_horizon", double_t, 0, "Future time to compute the control command", 0.2,0.01,10)
+splines.add("goal_dist", double_t, 0, "Maximum distance to the goal to check for gaol finished", 2.0,0.01,10.0)
+
+gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False)
exit(gen.generate(PACKAGE, "AckermannLocalPlannerAlgorithm", "AckermannLocalPlanner"))
diff --git a/include/ackermann_odom_helper.h b/include/ackermann_odom_helper.h
index 8f09f7c3ff4b3a172b7879fe0345b073087be594..de57bcb7f93b43dff72216ef06535606dcbcb5d3 100644
--- a/include/ackermann_odom_helper.h
+++ b/include/ackermann_odom_helper.h
@@ -42,6 +42,7 @@
#include <nav_msgs/Odometry.h>
#include <ackermann_msgs/AckermannDriveStamped.h>
#include <ackermann_msgs/AckermannDrive.h>
+#include <costmap_2d/costmap_2d_ros.h>
#include <ros/ros.h>
#include <boost/thread.hpp>
@@ -67,6 +68,7 @@ class AckermannOdomHelper
void set_odom_topic(const std::string &odom_topic);
void set_ackermann_topic(const std::string &ackermann_topic);
void set_average_samples(int num_average_samples);
+ void set_costmap(costmap_2d::Costmap2DROS* costmap_ros);
/** @brief Return the current odometry topic. */
std::string get_odom_topic(void) const;
@@ -96,6 +98,7 @@ class AckermannOdomHelper
ros::Subscriber odom_sub_;
nav_msgs::Odometry base_odom_;
boost::mutex odom_mutex_;
+ costmap_2d::Costmap2DROS* costmap_ros_;
ros::Subscriber ackermann_sub_;
std::vector<ackermann_msgs::AckermannDrive> ackermann_data_;
diff --git a/include/ackermann_planner.h b/include/ackermann_planner.h
index ba93e7247fd3de29f4e28551f19b0cf25a8b0b41..13cf62c303414b0ef1b181686341e23a765a885a 100644
--- a/include/ackermann_planner.h
+++ b/include/ackermann_planner.h
@@ -50,9 +50,9 @@
#include <costmap_2d/costmap_2d.h>
#include <base_local_planner/trajectory.h>
-#include <base_local_planner/local_planner_limits.h>
#include <base_local_planner/local_planner_util.h>
#include <ackermann_trajectory_generator.h>
+#include <ackermann_spline_generator.h>
#include <ackermann_odom_helper.h>
#include <base_local_planner/oscillation_cost_function.h>
@@ -88,15 +88,6 @@ class AckermannPlanner
*/
void reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg);
- /**
- * @brief Check if a trajectory is legal for a position/velocity pair
- * @param pos The robot's position
- * @param vel The robot's velocity
- * @param vel_samples The desired velocity
- * @return True if the trajectory is valid, false otherwise
- */
- bool check_trajectory(const Eigen::Vector3f pos,const Eigen::Vector3f ackermann,const Eigen::Vector2f samples);
-
/**
* @brief Given the current position and velocity of the robot, find the best trajectory to exectue
* @param global_pose The current position of the robot
@@ -110,13 +101,7 @@ class AckermannPlanner
* @brief Take in a new global plan for the local planner to follow, and adjust local costmaps
* @param new_plan The new global plan
*/
- void update_plan_and_local_costs(geometry_msgs::PoseStamped &global_pose,const std::vector<geometry_msgs::PoseStamped>& new_plan);
-
- /**
- * @brief Get the period at which the local planner is expected to run
- * @return The simulation period
- */
- double get_sim_period(void);
+ void update_plan_and_local_costs(geometry_msgs::PoseStamped &global_pose,const std::vector<geometry_msgs::PoseStamped>& new_plan,bool forward);
/**
* @brief Compute the components and total cost for a map grid cell
@@ -138,17 +123,14 @@ class AckermannPlanner
private:
AckermannPlannerUtil *planner_util_;
+ iri_ackermann_local_planner::AckermannLocalPlannerConfig current_config_;
- double stop_time_buffer_; ///< @brief How long before hitting something we're going to enforce that the robot stop
- double pdist_scale_, gdist_scale_, occdist_scale_,hdiff_scale_;
- Eigen::Vector2f vsamples_;
+ double pdist_scale_, gdist_scale_, occdist_scale_;
- double sim_period_;///< @brief The number of seconds to use to compute max/min vels for dwa
base_local_planner::Trajectory result_traj_;
- double forward_point_distance_;
-
std::vector<geometry_msgs::PoseStamped> global_plan_;
+ bool plan_forward;
boost::mutex configuration_mutex_;
std::string frame_id_;
@@ -156,12 +138,13 @@ class AckermannPlanner
bool publish_cost_grid_pc_; ///< @brief Whether or not to build and publish a PointCloud
bool publish_traj_pc_;
- double cheat_factor_;
+ ros::NodeHandle private_nh;
base_local_planner::MapGridVisualizer map_viz_; ///< @brief The map grid visualizer for outputting the potential field generated by the cost function
// see constructor body for explanations
AckermannTrajectoryGenerator generator_;
+ AckermannSplineGenerator spline_generator_;
base_local_planner::OscillationCostFunction oscillation_costs_;
base_local_planner::ObstacleCostFunction obstacle_costs_;
HeadingCostFunction heading_costs_;
diff --git a/include/ackermann_planner_limits.h b/include/ackermann_planner_limits.h
deleted file mode 100644
index 2907939a4ae5f3fe4728b7722964a64e0f7fe094..0000000000000000000000000000000000000000
--- a/include/ackermann_planner_limits.h
+++ /dev/null
@@ -1,130 +0,0 @@
-/***********************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * 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 Willow Garage 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 OWNER 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 _ACKERMANN_PLANNER_LIMITS_H
-#define _ACKERMANN_PLANNER_LIMITS_H
-
-#include <Eigen/Core>
-
-class AckermannPlannerLimits
-{
- public:
- // translational limits
- double max_trans_vel;
- double min_trans_vel;
- double max_trans_acc;
- // steering limits
- double max_steer_angle;
- double min_steer_angle;
- double max_steer_vel;
- double min_steer_vel;
- double max_steer_acc;
- // angular velocity limits
- double max_angular_vel;
- double min_angular_vel;
- // kinematic attributes
- double axis_distance;
- double wheel_distance;
- double wheel_radius;
- // general planner limits
- bool use_trans_vel_deadzone;
- double trans_vel_deadzone;
- double split_ignore_length;
- bool prune_plan;
- double xy_goal_tolerance;
- double yaw_goal_tolerance;
- double trans_stopped_vel;
- double rot_stopped_vel;
- bool restore_defaults;
-
- AckermannPlannerLimits() {}
-
- AckermannPlannerLimits(
- // translational limits
- double max_trans_vel,
- double min_trans_vel,
- double max_trans_acc,
- // steering limits
- double max_steer_angle,
- double min_steer_angle,
- double max_steer_vel,
- double min_steer_vel,
- double max_steer_acc,
- // angular velocity limits
- double max_angular_vel,
- double min_angular_vel,
- // kinematic attributes
- double axis_distance,
- double wheel_distance,
- double wheel_radius,
- // general planner limits
- bool use_trans_vel_deadzone,
- double trans_vel_deadzone,
- double split_ignore_length,
- double xy_goal_tolerance,
- double yaw_goal_tolerance,
- bool prune_plan=true,
- double trans_stopped_vel=0.1,
- double rot_stopped_vel=0.1):
- max_trans_vel(max_trans_vel),
- min_trans_vel(min_trans_vel),
- max_trans_acc(max_trans_acc),
- max_steer_angle(max_steer_angle),
- min_steer_angle(min_steer_angle),
- max_steer_vel(max_steer_vel),
- min_steer_vel(min_steer_vel),
- max_steer_acc(max_steer_acc),
- max_angular_vel(max_angular_vel),
- min_angular_vel(min_angular_vel),
- axis_distance(axis_distance),
- wheel_distance(wheel_distance),
- wheel_radius(wheel_radius),
- use_trans_vel_deadzone(use_trans_vel_deadzone),
- trans_vel_deadzone(trans_vel_deadzone),
- split_ignore_length(split_ignore_length),
- prune_plan(prune_plan),
- xy_goal_tolerance(xy_goal_tolerance),
- yaw_goal_tolerance(yaw_goal_tolerance),
- trans_stopped_vel(trans_stopped_vel),
- rot_stopped_vel(rot_stopped_vel)
- {
- }
-
- ~AckermannPlannerLimits()
- {
- }
-};
-
-#endif // __LOCALPLANNERLIMITS_H__
diff --git a/include/ackermann_planner_ros.h b/include/ackermann_planner_ros.h
index 8eb35b9edf74f3a2e342d55fe689205b3f1094f2..c85ff95cbd4bb1cf72c8f8e0679ff46508ca06c6 100644
--- a/include/ackermann_planner_ros.h
+++ b/include/ackermann_planner_ros.h
@@ -136,10 +136,9 @@ class AckermannPlannerROS : public nav_core::BaseLocalPlanner
dynamic_reconfigure::Server<iri_ackermann_local_planner::AckermannLocalPlannerConfig> *dsrv_;
iri_ackermann_local_planner::AckermannLocalPlannerConfig default_config_;
- bool setup_;
+ iri_ackermann_local_planner::AckermannLocalPlannerConfig current_config_;
geometry_msgs::PoseStamped current_pose_;
bool initialized_;
- int patience_;
AckermannOdomHelper odom_helper_;
std::string odom_topic_;
@@ -147,13 +146,5 @@ class AckermannPlannerROS : public nav_core::BaseLocalPlanner
bool stuck;
bool first;
- bool use_steer_angle_cmd;
- bool invert_steer_angle_cmd;
- bool use_stopped_steering;
- double stopped_steering_angle_threshold;
- bool use_stuck_check;
- double stuck_check_vel_threshold;
- int stuck_check_max_occurrences;
-
};
#endif
diff --git a/include/ackermann_planner_util.h b/include/ackermann_planner_util.h
index 393d3a3b01095fe3626651cf65134ab5391c1d33..1a32bb697a9db2f220bcec52b2f5869a822c6c8c 100644
--- a/include/ackermann_planner_util.h
+++ b/include/ackermann_planner_util.h
@@ -45,10 +45,9 @@
#include <costmap_2d/costmap_2d.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
+#include <base_local_planner/costmap_model.h>
-#include <ackermann_planner_limits.h>
-
-
+#include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
/**
* @class AckermannPlannerUtil
* @brief Helper class implementing infrastructure code many local planner implementations may need.
@@ -61,27 +60,29 @@ class AckermannPlannerUtil
std::string global_frame_;
costmap_2d::Costmap2D* costmap_;
+ base_local_planner::CostmapModel *world_model_;
tf2_ros::Buffer *tf_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
+ bool plan_forward;
- boost::mutex limits_configuration_mutex_;
- bool setup_;
- AckermannPlannerLimits default_limits_;
- AckermannPlannerLimits limits_;
bool initialized_;
std::vector < std::vector<geometry_msgs::PoseStamped> > subPathList;
std::vector < geometry_msgs::PoseStamped > subPath;
+ std::vector < bool > forward_segment;
unsigned int subPathIndex;
+ iri_ackermann_local_planner::AckermannLocalPlannerConfig default_config_;
+ iri_ackermann_local_planner::AckermannLocalPlannerConfig current_config_;
+
public:
AckermannPlannerUtil();
/**
* @brief Callback to update the local planner's parameters
*/
- void reconfigure_callback(AckermannPlannerLimits &config, bool restore_defaults);
+ void reconfigure_callback(iri_ackermann_local_planner::AckermannLocalPlannerConfig &config, bool restore_defaults);
void initialize(tf2_ros::Buffer* tf,costmap_2d::Costmap2D* costmap,std::string global_frame);
@@ -93,11 +94,11 @@ class AckermannPlannerUtil
bool last_path(void);
- bool get_local_plan(geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan);
+ bool get_local_plan(geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan, bool &forward);
costmap_2d::Costmap2D* get_costmap();
- AckermannPlannerLimits get_current_limits();
+ base_local_planner::CostmapModel *get_world_model();
std::string get_global_frame(void);
diff --git a/include/ackermann_spline_generator.h b/include/ackermann_spline_generator.h
new file mode 100644
index 0000000000000000000000000000000000000000..05b65687c80d0c2927a9c4a52b253b5ad4abc864
--- /dev/null
+++ b/include/ackermann_spline_generator.h
@@ -0,0 +1,100 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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 OWNER 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.
+ *
+ * Author: TKruse
+ *********************************************************************/
+
+#ifndef ACKERMANN_SPLINE_GENERATOR_H_
+#define ACKERMANN_SPLINE_GENERATOR_H_
+
+#include <base_local_planner/trajectory_sample_generator.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <Eigen/Core>
+
+#include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
+
+#include "g2_spline.h"
+#include "vel_trapezoid.h"
+
+typedef struct
+{
+ Eigen::Vector4f params;
+ TPoint end_point;
+}TSplineSample;
+
+/**
+ */
+class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGenerator
+{
+ public:
+
+ AckermannSplineGenerator();
+
+ /**
+ * @brief Reconfigures the trajectory planner
+ */
+ void reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg);
+
+ /**
+ * Whether this generator can create more trajectories
+ */
+ bool hasMoreTrajectories(void);
+
+ /**
+ * Whether this generator can create more trajectories
+ */
+ bool nextTrajectory(base_local_planner::Trajectory &traj);
+
+ void initialise(const Eigen::Vector3f& pos,const Eigen::Vector3f& ackermann,const Eigen::Vector3f& goal,bool forward,std::vector<geometry_msgs::PoseStamped>& current_plan);
+
+ void set_sim_period(double period);
+
+ ~AckermannSplineGenerator();
+ protected:
+ std::vector<TSplineSample> samples_;
+ CG2Spline spline_;
+ unsigned int next_sample_index_;
+ iri_ackermann_local_planner::AckermannLocalPlannerConfig current_config_;
+ Eigen::Vector3f pos_;
+ Eigen::Vector3f goal_pos_;
+ Eigen::Vector3f ackermann_;
+ std::vector<geometry_msgs::PoseStamped> current_plan;
+ bool forward;
+ double sim_period_;
+ double y_offset;
+
+ unsigned int get_path_closest_pose(double &path_x,double &path_y);
+};
+
+#endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */
diff --git a/include/ackermann_trajectory_generator.h b/include/ackermann_trajectory_generator.h
index 26e6a3f6a883c7f9a8ec7ae1e54922c857abf60e..5e6cfb9d3edb71508202bba9782909f13e05da48 100644
--- a/include/ackermann_trajectory_generator.h
+++ b/include/ackermann_trajectory_generator.h
@@ -39,9 +39,10 @@
#define ACKERMANN_TRAJECTORY_GENERATOR_H_
#include <base_local_planner/trajectory_sample_generator.h>
-#include <ackermann_planner_limits.h>
#include <Eigen/Core>
+#include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
+
/**
* generates trajectories based on equi-distant discretisation of the degrees of freedom.
* This is supposed to be a simple and robust implementation of the TrajectorySampleGenerator
@@ -71,28 +72,14 @@ class AckermannTrajectoryGenerator: public base_local_planner::TrajectorySampleG
* @param discretize_by_time if true, the trajectory is split according in chunks of the same duration, else of same length
*/
void initialise(
- const Eigen::Vector3f& pos,
- const Eigen::Vector3f& ackermann,
- const Eigen::Vector3f& goal,
- AckermannPlannerLimits* limits,
- const Eigen::Vector2f& vsamples,
- bool discretize_by_time = false);
+ const Eigen::Vector3f& pos,
+ const Eigen::Vector3f& ackermann,
+ const Eigen::Vector3f& goal);
/**
- * This function is to be called only when parameters change
- *
- * @param sim_granularity granularity of collision detection
- * @param angular_sim_granularity angular granularity of collision detection
- * @param use_dwa whether to use DWA or trajectory rollout
- * @param sim_period distance between points in one trajectory
+ * @brief Reconfigures the trajectory planner
*/
- void set_parameters(
- double max_sim_time,
- double min_sim_time,
- double sim_granularity,
- double angular_sim_granularity,
- bool use_steer_angle_cmd,
- double sim_period = 0.0);
+ void reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg);
/**
* Whether this generator can create more trajectories
@@ -106,10 +93,10 @@ class AckermannTrajectoryGenerator: public base_local_planner::TrajectorySampleG
bool generate_trajectory(
- Eigen::Vector3f pos,
- Eigen::Vector3f vel,
- Eigen::Vector2f sample_target_vel,
- base_local_planner::Trajectory& traj);
+ Eigen::Vector3f pos,
+ Eigen::Vector3f vel,
+ Eigen::Vector2f sample_target_vel,
+ base_local_planner::Trajectory& traj);
~AckermannTrajectoryGenerator();
protected:
@@ -117,17 +104,11 @@ class AckermannTrajectoryGenerator: public base_local_planner::TrajectorySampleG
unsigned int next_sample_index_;
// to store sample params of each sample between init and generation
std::vector<Eigen::Vector2f> sample_params_;
- AckermannPlannerLimits* limits_;
+ iri_ackermann_local_planner::AckermannLocalPlannerConfig current_config_;
Eigen::Vector3f pos_;
Eigen::Vector3f ackermann_;
- // whether velocity of trajectory changes over time or not
- bool discretize_by_time_;
-
- double max_sim_time_,min_sim_time_,sim_time_;
- double sim_granularity_, angular_sim_granularity_;
- double sim_period_; // only for dwa
- bool use_steer_angle_cmd_;
+ double sim_time_;
};
#endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */
diff --git a/include/ackermann_trajectory_search.h b/include/ackermann_trajectory_search.h
index 7c688efc38643740027c67d0026d72c331ca86c4..822397e5fbc208cc933f8f591803967757810184 100644
--- a/include/ackermann_trajectory_search.h
+++ b/include/ackermann_trajectory_search.h
@@ -43,6 +43,9 @@
#include <base_local_planner/trajectory_cost_function.h>
#include <base_local_planner/trajectory_sample_generator.h>
#include <base_local_planner/trajectory_search.h>
+#include <ackermann_spline_generator.h>
+
+#include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
/**
* @class AckermannTrajectorySearch
@@ -91,7 +94,6 @@ public:
*/
bool findBestTrajectory(base_local_planner::Trajectory& traj, std::vector<base_local_planner::Trajectory>* all_explored = 0);
-
private:
std::vector<base_local_planner::TrajectorySampleGenerator*> gen_list_;
std::vector<base_local_planner::TrajectoryCostFunction*> critics_;
diff --git a/params/ackermann_planner_params.yaml b/params/ackermann_planner_params.yaml
index ab581039c0fc6c37279ea2ec352f548efc61324f..09818d7e211e3423c61ab5c934c54be83a0a86c9 100644
--- a/params/ackermann_planner_params.yaml
+++ b/params/ackermann_planner_params.yaml
@@ -13,6 +13,8 @@ AckermannPlannerROS:
oscillation_reset_dist: 0.002
oscillation_reset_angle: 0.001
+ check_path_r_inc: 0.25
+
trans_vel_samples: 31
steer_angle_samples: 21
angular_vel_samples: 21
diff --git a/params/ackermann_planner_params_fixed_gplanner.yaml b/params/ackermann_planner_params_fixed_gplanner.yaml
index 5ea2abed65ec37e065e8c7e8a4464b090e455ea2..1c9fad3fc51348ec4c6825a89d34b82661300b74 100644
--- a/params/ackermann_planner_params_fixed_gplanner.yaml
+++ b/params/ackermann_planner_params_fixed_gplanner.yaml
@@ -17,6 +17,8 @@ AckermannPlannerROS:
# scaling_speed:
# max_scaling_factor:
+ check_path_r_inc: 0.25
+
trans_vel_samples: 21
steer_angle_samples: 11
diff --git a/src/ackermann_odom_helper.cpp b/src/ackermann_odom_helper.cpp
index 5056a825c74f61c9f8fb6a0e6d8eb781e51add5e..9276b8ee6b8c047aa217c602f3ce1518f7288cf8 100644
--- a/src/ackermann_odom_helper.cpp
+++ b/src/ackermann_odom_helper.cpp
@@ -41,15 +41,26 @@ AckermannOdomHelper::AckermannOdomHelper(const std::string &odom_topic,const std
set_odom_topic(odom_topic);
set_ackermann_topic(ackermann_topic);
ackermann_data_.resize(num_avg_samples);
+ this->costmap_ros_=NULL;
}
void AckermannOdomHelper::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
+ geometry_msgs::PoseStamped pose_in;
ROS_INFO_ONCE("Odom received!");
-
+
//we assume that the odometry is published in the frame of the base
boost::mutex::scoped_lock lock(odom_mutex_);
base_odom_=*msg;
+ if(this->costmap_ros_!=NULL)
+ {
+ if(!this->costmap_ros_->getRobotPose(pose_in))
+ {
+ ROS_ERROR("AckermannOdomHelper: Could not get robot pose");
+ return;
+ }
+ base_odom_.pose.pose=pose_in.pose;
+ }
}
void AckermannOdomHelper::ackermann_callback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg)
@@ -147,6 +158,12 @@ void AckermannOdomHelper::set_average_samples(int num_average_samples)
this->ackermann_data_.resize(num_average_samples);
}
+void AckermannOdomHelper::set_costmap(costmap_2d::Costmap2DROS* costmap_ros)
+{
+ boost::mutex::scoped_lock lock(odom_mutex_);
+ this->costmap_ros_=costmap_ros;
+}
+
std::string AckermannOdomHelper::get_odom_topic(void) const
{
return this->odom_topic_;
diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index 371634056d22b0706be91c41ba8e31dc9b5c266a..1847eecd2998f4685804904242f57c65ddf89ff8 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -51,94 +51,108 @@
void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &config)
{
boost::mutex::scoped_lock l(configuration_mutex_);
-
- generator_.set_parameters(
- config.max_sim_time,
- config.min_sim_time,
- config.sim_granularity,
- config.angular_sim_granularity,
- config.use_steer_angle_cmd,
- sim_period_);
+ double hdiff_scale;
double resolution = planner_util_->get_costmap()->getResolution();
pdist_scale_ = config.path_distance_bias;
// pdistscale used for both path and alignment, set forward_point_distance to zero to discard alignment
- path_costs_.setScale(resolution * pdist_scale_ * 0.5);
+ path_costs_.setScale(resolution * pdist_scale_);
//path_costs_.setStopOnFailure(false);
- hdiff_scale_=config.hdiff_scale;
- heading_costs_.setScale(hdiff_scale_);
+ hdiff_scale=config.hdiff_scale;
+ heading_costs_.setScale(hdiff_scale);
+ heading_costs_.set_num_points(config.heading_points);
gdist_scale_ = config.goal_distance_bias;
- goal_costs_.setScale(resolution * gdist_scale_ * 0.5);
+ goal_costs_.setScale(resolution * gdist_scale_);
goal_costs_.setStopOnFailure(false);
occdist_scale_ = config.occdist_scale;
obstacle_costs_.setScale(resolution * occdist_scale_);
- stop_time_buffer_ = config.stop_time_buffer;
oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle);
- forward_point_distance_ = config.forward_point_distance;
- heading_costs_.set_num_points(config.heading_points);
// obstacle costs can vary due to scaling footprint feature
obstacle_costs_.setParams(config.max_trans_vel, config.max_scaling_factor, config.scaling_speed);
- int trans_vel_samp, steer_angle_angular_vel_samp;
- trans_vel_samp = config.trans_vel_samples;
- if(config.use_steer_angle_cmd)
- steer_angle_angular_vel_samp = config.steer_angle_samples;
+ std::string controller_frequency_param_name;
+ double sim_period;
+ if(!this->private_nh.searchParam("controller_frequency", controller_frequency_param_name))
+ {
+ ROS_WARN("AckermannPlanner::reconfigure_callback -> Parameter controller_frequency not found. Assuming a rate of 20Hz");
+ sim_period = 0.05;
+ }
else
- steer_angle_angular_vel_samp = config.angular_vel_samples;
-
- if (trans_vel_samp <= 0)
{
- ROS_WARN("You've specified that you don't want any samples in the translational dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
- trans_vel_samp = 1;
- config.trans_vel_samples = trans_vel_samp;
+ double controller_frequency = 0;
+ this->private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
+ if(controller_frequency > 0)
+ sim_period = 1.0 / controller_frequency;
+ else
+ {
+ ROS_WARN("AckermannPlanner::reconfigure_callback -> A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
+ sim_period = 0.05;
+ }
}
- if (steer_angle_angular_vel_samp <= 0)
+ // set up all the cost functions that will be applied in order
+ // (any function returning negative values will abort scoring, so the order can improve performance)
+ std::vector<base_local_planner::TrajectoryCostFunction*> critics;
+ std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
+ if(config.use_splines)
{
- ROS_WARN("You've specified that you don't want any samples in the steering/angular velocity dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
- steer_angle_angular_vel_samp = 1;
- if(config.use_steer_angle_cmd)
- config.steer_angle_samples = steer_angle_angular_vel_samp;
- else
- config.angular_vel_samples = steer_angle_angular_vel_samp;
+ critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
+ critics.push_back(&path_costs_); // prefers trajectories on global path
+ critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+ critics.push_back(&heading_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+
+ // trajectory generators
+ generator_list.push_back(&spline_generator_);
+ spline_generator_.set_sim_period(sim_period);
}
+ else
+ {
+ critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
+ critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
+ critics.push_back(&path_costs_); // prefers trajectories on global path
+ critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+ critics.push_back(&heading_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+
+ // trajectory generators
+ generator_list.push_back(&generator_);
+ }
+
+ scored_sampling_planner_ = AckermannTrajectorySearch(generator_list, critics);
- vsamples_[0] = trans_vel_samp;
- vsamples_[1] = steer_angle_angular_vel_samp;
+ generator_.reconfigure(config);
+ spline_generator_.reconfigure(config);
+
+ current_config_=config;
}
AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planner_util) :
planner_util_(planner_util),
obstacle_costs_(planner_util->get_costmap()),
- path_costs_(planner_util->get_costmap()),
- goal_costs_(planner_util->get_costmap(), 0.0, 0.0, true)
+ path_costs_(planner_util->get_costmap(), 0.0, 0.0, false,base_local_planner::Sum),
+ goal_costs_(planner_util->get_costmap(), 0.0, 0.0, true),
+ private_nh("~/" + name)
{
- ros::NodeHandle private_nh("~/" + name);
-
- //Assuming this planner is being run within the navigation stack, we can
- //just do an upward search for the frequency at which its being run. This
- //also allows the frequency to be overwritten locally.
std::string controller_frequency_param_name;
- if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name))
+ double sim_period;
+ if(!this->private_nh.searchParam("controller_frequency", controller_frequency_param_name))
{
- sim_period_ = 0.05;
- }
- else
+ ROS_WARN_STREAM("AckermannPlanner::AckermannPlanner -> Parameter controller_frequency not found at ns " << this->private_nh.getNamespace() << ". Assuming a rate of 20Hz");
+ sim_period = 0.05;
+ }
+ else
{
double controller_frequency = 0;
- private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
- if(controller_frequency > 0)
- {
- sim_period_ = 1.0 / controller_frequency;
- }
- else
+ this->private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
+ if(controller_frequency > 0)
+ sim_period = 1.0 / controller_frequency;
+ else
{
ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
- sim_period_ = 0.05;
+ sim_period = 0.05;
}
}
@@ -159,19 +173,33 @@ AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planne
// set up all the cost functions that will be applied in order
// (any function returning negative values will abort scoring, so the order can improve performance)
std::vector<base_local_planner::TrajectoryCostFunction*> critics;
- critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
- critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
- critics.push_back(&path_costs_); // prefers trajectories on global path
- critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
- critics.push_back(&heading_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
-
- // trajectory generators
+ bool use_splines;
+ private_nh.param("use_splines", use_splines, false);
std::vector<base_local_planner::TrajectorySampleGenerator*> generator_list;
- generator_list.push_back(&generator_);
+ if(use_splines)
+ {
+ critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
+ critics.push_back(&path_costs_); // prefers trajectories on global path
+ critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+ critics.push_back(&heading_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+
+ // trajectory generators
+ generator_list.push_back(&spline_generator_);
+ spline_generator_.set_sim_period(sim_period);
+ }
+ else
+ {
+ critics.push_back(&oscillation_costs_); // discards oscillating motions (assisgns cost -1)
+ critics.push_back(&obstacle_costs_); // discards trajectories that move into obstacles
+ critics.push_back(&path_costs_); // prefers trajectories on global path
+ critics.push_back(&goal_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+ critics.push_back(&heading_costs_); // prefers trajectories that go towards (local) goal, based on wave propagation
+
+ // trajectory generators
+ generator_list.push_back(&generator_);
+ }
scored_sampling_planner_ = AckermannTrajectorySearch(generator_list, critics);
-
- private_nh.param("cheat_factor", cheat_factor_, 1.0);
}
// used for visualization only, total_costs are not really total costs
@@ -199,50 +227,19 @@ bool AckermannPlanner::set_plan(const std::vector<geometry_msgs::PoseStamped>& o
return planner_util_->set_plan(orig_global_plan);
}
-/**
- * This function is used when other strategies are to be applied,
- * but the cost functions for obstacles are to be reused.
- */
-bool AckermannPlanner::check_trajectory(Eigen::Vector3f pos,Eigen::Vector3f ackermann,Eigen::Vector2f samples)
-{
- oscillation_costs_.resetOscillationFlags();
- base_local_planner::Trajectory traj;
- geometry_msgs::PoseStamped goal_pose = global_plan_.back();
- Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
- AckermannPlannerLimits limits = planner_util_->get_current_limits();
- generator_.initialise(pos,
- ackermann,
- goal,
- &limits,
- vsamples_);
- generator_.generate_trajectory(pos, ackermann, samples, traj);
- double cost = scored_sampling_planner_.scoreTrajectory(traj, -1);
- //if the trajectory is a legal one... the check passes
- if(cost >= 0)
- return true;
- ROS_WARN("Invalid Trajectory %f, %f, cost: %f", samples[0], samples[1], cost);
-
- //otherwise the check fails
- return false;
-}
-
-void AckermannPlanner::update_plan_and_local_costs(geometry_msgs::PoseStamped &global_pose,const std::vector<geometry_msgs::PoseStamped>& new_plan)
+void AckermannPlanner::update_plan_and_local_costs(geometry_msgs::PoseStamped &global_pose,const std::vector<geometry_msgs::PoseStamped>& new_plan,bool forward)
{
global_plan_.resize(new_plan.size());
for (unsigned int i = 0; i < new_plan.size(); ++i)
global_plan_[i] = new_plan[i];
+ plan_forward=forward;
// costs for going away from path
path_costs_.setTargetPoses(global_plan_);
-
// costs for not going towards the local goal as much as possible
goal_costs_.setTargetPoses(global_plan_);
heading_costs_.set_global_plan(global_plan_);
-
- // alignment costs
- geometry_msgs::PoseStamped goal_pose = global_plan_.back();
-
}
/*
@@ -250,6 +247,11 @@ void AckermannPlanner::update_plan_and_local_costs(geometry_msgs::PoseStamped &g
*/
base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::PoseStamped &global_pose,ackermann_msgs::AckermannDrive &ackermann,geometry_msgs::PoseStamped& drive_velocities,std::vector<geometry_msgs::Point> footprint_spec)
{
+ std::vector<geometry_msgs::Point> circular_footprint;
+ geometry_msgs::PoseStamped goal_pose;
+ double min_radius,max_radius;
+ unsigned int path_index=global_plan_.size()-1;
+
obstacle_costs_.setFootprint(footprint_spec);
//make sure that our configuration doesn't change mid-run
@@ -257,21 +259,52 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y, tf::getYaw(global_pose.pose.orientation));
Eigen::Vector3f ack_state(ackermann.speed,ackermann.steering_angle,ackermann.steering_angle_velocity);
- geometry_msgs::PoseStamped goal_pose = global_plan_.back();
+ costmap_2d::calculateMinAndMaxDistances(footprint_spec,min_radius,max_radius);
+ for(double angle=0;angle<2*3.14159;angle+=0.1)
+ {
+ geometry_msgs::Point new_point;
+ new_point.x=(max_radius+this->current_config_.check_path_r_inc)*cos(angle);
+ new_point.y=(max_radius+this->current_config_.check_path_r_inc)*sin(angle);
+ circular_footprint.push_back(new_point);
+ }
+ do{
+ if(planner_util_->get_world_model()->footprintCost(global_plan_[path_index].pose.position.x,global_plan_[path_index].pose.position.y,tf::getYaw(global_plan_[path_index].pose.orientation),circular_footprint)<0)
+ {
+ if(path_index>0)
+ path_index--;
+ continue;
+ }
+ else
+ break;
+ }while(path_index>0);
+ if(path_index==0)
+ {
+ std::cout << "empty path" << std::endl;
+ drive_velocities.pose.position.x = 0;
+ drive_velocities.pose.position.y = 0;
+ drive_velocities.pose.position.z = 0;
+ drive_velocities.pose.orientation.w = 1;
+ drive_velocities.pose.orientation.x = 0;
+ drive_velocities.pose.orientation.y = 0;
+ drive_velocities.pose.orientation.z = 0;
+ return base_local_planner::Trajectory();
+ }
+ else
+ goal_pose = global_plan_[path_index];
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
- AckermannPlannerLimits limits = planner_util_->get_current_limits();
+
+ std::vector<geometry_msgs::PoseStamped> plan_fixed(path_index+1);
+ for (unsigned int i=0; i<=path_index; i++)
+ plan_fixed[i] = global_plan_[i];
// prepare cost functions and generators for this run
- generator_.initialise(pos,
- ack_state,
- goal,
- &limits,
- vsamples_);
+ generator_.initialise(pos,ack_state,goal);
+ spline_generator_.initialise(pos,ack_state,goal,plan_forward,plan_fixed);
result_traj_.cost_ = -7;
// find best trajectory by sampling and scoring the samples
std::vector<base_local_planner::Trajectory> all_explored;
- scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
+ scored_sampling_planner_.findBestTrajectory(result_traj_, (publish_traj_pc_? &all_explored: NULL));
if(publish_traj_pc_)
{
@@ -289,8 +322,8 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
unsigned int num_points = 0;
for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
{
- if (t->cost_<0)
- continue;
+// if (t->cost_<0)
+// continue;
num_points += t->getPointsSize();
}
@@ -298,8 +331,8 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
sensor_msgs::PointCloud2Iterator<float> iter_x(traj_cloud, "x");
for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
{
- if(t->cost_<0)
- continue;
+// if(t->cost_<0)
+// continue;
// Fill out the plan
for(unsigned int i = 0; i < t->getPointsSize(); ++i)
{
@@ -324,11 +357,12 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
}
// debrief stateful scoring functions
- oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->get_current_limits().min_trans_vel);
+ oscillation_costs_.updateOscillationFlags(pos, &result_traj_, current_config_.min_trans_vel);
//if we don't have a legal trajectory, we'll just command zero
if (result_traj_.cost_ < 0)
{
+ std::cout << "no legal trajectory" << std::endl;
drive_velocities.pose.position.x = 0;
drive_velocities.pose.position.y = 0;
drive_velocities.pose.position.z = 0;
@@ -339,12 +373,20 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
}
else
{
+ std::cout << result_traj_.xv_ << "," << result_traj_.thetav_ << std::endl;
drive_velocities.pose.position.x = result_traj_.xv_;
drive_velocities.pose.position.y = result_traj_.yv_;
- drive_velocities.pose.position.z = 0;
+ drive_velocities.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, result_traj_.thetav_);
tf2::convert(q, drive_velocities.pose.orientation);
+
+// drive_velocities.pose.position.x = 0.0;
+// drive_velocities.pose.position.y = 0.0;
+// drive_velocities.pose.position.z = 0.0;
+// tf2::Quaternion q;
+// q.setRPY(0, 0, 0);
+// tf2::convert(q, drive_velocities.pose.orientation);
}
return result_traj_;
diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index c770851c21a041f78058b9c8f749d4cf4e1dcf70..76c23a8b2d472409c162e7ad9efbbefa5c5cb009 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/src/ackermann_planner_ros.cpp
@@ -52,65 +52,32 @@
PLUGINLIB_EXPORT_CLASS(AckermannPlannerROS, nav_core::BaseLocalPlanner)
AckermannPlannerROS::AckermannPlannerROS() : initialized_(false),
- odom_helper_("odom","ackermann_feedback",10), setup_(false)
+ odom_helper_("odom","ackermann_feedback",10)
{
- patience_=1;
this->stuck=false;
this->first=true;
}
void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::AckermannLocalPlannerConfig &config, uint32_t level)
{
- if (setup_ && config.restore_defaults)
+ static bool setup=false;
+
+ if (setup && config.restore_defaults)
{
config = default_config_;
config.restore_defaults = false;
}
- if ( ! setup_)
+ if (!setup)
{
default_config_ = config;
- setup_ = true;
+ setup = true;
}
- // update generic local planner params
- AckermannPlannerLimits limits;
- limits.max_trans_vel = config.max_trans_vel;
- limits.min_trans_vel = config.min_trans_vel;
- limits.max_trans_acc = config.max_trans_acc;
- limits.max_steer_angle = config.max_steer_angle;
- limits.min_steer_angle = config.min_steer_angle;
- limits.max_steer_vel = config.max_steer_vel;
- limits.min_steer_vel = config.min_steer_vel;
- limits.max_steer_acc = config.max_steer_acc;
- limits.max_angular_vel = config.max_angular_vel;
- limits.min_angular_vel = config.min_angular_vel;
- // kinematic attributes
- limits.axis_distance = config.axis_distance;
- limits.wheel_distance = config.wheel_distance;
- limits.wheel_radius = config.wheel_radius;
- // general planner limits
- limits.use_trans_vel_deadzone = config.use_trans_vel_deadzone;
- limits.trans_vel_deadzone = config.trans_vel_deadzone;
- limits.split_ignore_length = config.split_ignore_length;
- limits.xy_goal_tolerance = config.xy_goal_tolerance;
- limits.yaw_goal_tolerance = config.yaw_goal_tolerance;
- limits.prune_plan = config.prune_plan;
- limits.trans_stopped_vel = config.trans_stopped_vel;
- limits.rot_stopped_vel = config.rot_stopped_vel;
- planner_util_.reconfigure_callback(limits, config.restore_defaults);
+ planner_util_.reconfigure_callback(config, config.restore_defaults);
this->last_cmds.resize(config.cmd_vel_avg);
- this->use_steer_angle_cmd=config.use_steer_angle_cmd;
- this->invert_steer_angle_cmd=config.invert_steer_angle_cmd;
- this->use_stopped_steering=config.use_stopped_steering;
-
- this->stopped_steering_angle_threshold=config.stopped_steering_angle_threshold;
- this->use_stuck_check = config.use_stuck_check;
- this->stuck_check_vel_threshold=config.stuck_check_vel_threshold;
- this->stuck_check_max_occurrences=config.stuck_check_max_occurrences;
-
odom_helper_.set_average_samples(config.odom_avg);
- patience_=config.planner_patience;
+ current_config_=config;
// update ackermann specific configuration
dp_->reconfigure(config);
@@ -136,13 +103,16 @@ void AckermannPlannerROS::initialize(std::string name,tf2_ros::Buffer* tf,costma
dp_ = boost::shared_ptr<AckermannPlanner>(new AckermannPlanner(name, &planner_util_));
if( private_nh.getParam( "odom_topic", odom_topic_ ))
- {
odom_helper_.set_odom_topic( odom_topic_ );
- }
+ else
+ ROS_WARN_STREAM("AckermannPlannerROS::initialize -> Parameter odom_topic not found at ns " << private_nh.getNamespace() << "");
+
if( private_nh.getParam( "ackermann_topic", ackermann_topic_ ))
- {
odom_helper_.set_ackermann_topic( ackermann_topic_ );
- }
+ else
+ ROS_WARN_STREAM("AckermannPlannerROS::initialize -> Parameter ackermann_topic not found at ns " << private_nh.getNamespace() << "");
+
+ odom_helper_.set_costmap(costmap_ros_);
initialized_ = true;
@@ -153,9 +123,7 @@ void AckermannPlannerROS::initialize(std::string name,tf2_ros::Buffer* tf,costma
this->first=true;
}
else
- {
ROS_WARN("AckermannPlannerROS: this planner has already been initialized, doing nothing.");
- }
}
bool AckermannPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
@@ -195,23 +163,21 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
}
ackermann_msgs::AckermannDrive ackermann;
- AckermannPlannerLimits limits;
-
odom_helper_.get_ackermann_state(ackermann);
- limits=planner_util_.get_current_limits();
+
// saturate speeds and angles
- if(ackermann.speed>limits.max_trans_vel)
- ackermann.speed=limits.max_trans_vel;
- else if(ackermann.speed<limits.min_trans_vel)
- ackermann.speed=limits.min_trans_vel;
- if(ackermann.steering_angle>limits.max_steer_angle)
- ackermann.steering_angle=limits.max_steer_angle;
- else if(ackermann.steering_angle<limits.min_steer_angle)
- ackermann.steering_angle=limits.min_steer_angle;
- if(ackermann.steering_angle_velocity>limits.max_steer_vel)
- ackermann.steering_angle_velocity=limits.max_steer_vel;
- else if(ackermann.steering_angle_velocity<limits.min_steer_vel)
- ackermann.steering_angle_velocity=limits.min_steer_vel;
+ if(ackermann.speed>current_config_.max_trans_vel)
+ ackermann.speed=current_config_.max_trans_vel;
+ else if(ackermann.speed<current_config_.min_trans_vel)
+ ackermann.speed=current_config_.min_trans_vel;
+ if(ackermann.steering_angle>current_config_.max_steer_angle)
+ ackermann.steering_angle=current_config_.max_steer_angle;
+ else if(ackermann.steering_angle<current_config_.min_steer_angle)
+ ackermann.steering_angle=current_config_.min_steer_angle;
+ if(ackermann.steering_angle_velocity>current_config_.max_steer_vel)
+ ackermann.steering_angle_velocity=current_config_.max_steer_vel;
+ else if(ackermann.steering_angle_velocity<current_config_.min_steer_vel)
+ ackermann.steering_angle_velocity=current_config_.min_steer_vel;
//compute what trajectory to drive along
geometry_msgs::PoseStamped drive_cmds;
@@ -224,7 +190,7 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
//pass along drive commands
cmd_vel.linear.x = drive_cmds.pose.position.x;
cmd_vel.linear.y = drive_cmds.pose.position.y;
- if(this->invert_steer_angle_cmd)
+ if(current_config_.invert_steer_angle_cmd)
cmd_vel.angular.z = -tf::getYaw(drive_cmds.pose.orientation);
else
cmd_vel.angular.z = tf::getYaw(drive_cmds.pose.orientation);
@@ -238,7 +204,7 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
local_plan.clear();
publish_local_plan(local_plan);
count++;
- if(count>patience_)
+ if(count>current_config_.planner_patience)
{
count=0;
return false;
@@ -278,7 +244,7 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
bool AckermannPlannerROS::isGoalReached(void)
{
nav_msgs::Odometry odom;
- AckermannPlannerLimits limits;
+ bool forward;
if (! is_initialized()) {
ROS_ERROR("AckermannPlannerROS: this planner has not been initialized, please call initialize() before using this planner");
@@ -289,13 +255,18 @@ bool AckermannPlannerROS::isGoalReached(void)
return false;
}
std::vector<geometry_msgs::PoseStamped> transformed_plan;
- if ( ! planner_util_.get_local_plan(current_pose_, transformed_plan))
+ if ( ! planner_util_.get_local_plan(current_pose_, transformed_plan,forward))
{
ROS_ERROR("AckermannPlannerROS: could not get local plan");
return false;
}
+ if(transformed_plan.empty())
+ {
+ ROS_WARN_NAMED("AckermannPlannerROS", "Received an empty transformed plan.");
+ return false;
+ }
+
odom_helper_.get_odom(odom);
- limits=planner_util_.get_current_limits();
if(planner_util_.last_path())
{
if(base_local_planner::isGoalReached(*tf_,
@@ -304,8 +275,8 @@ bool AckermannPlannerROS::isGoalReached(void)
costmap_ros_->getGlobalFrameID(),
current_pose_,
odom,
- limits.rot_stopped_vel,limits.trans_stopped_vel,
- limits.xy_goal_tolerance,limits.yaw_goal_tolerance))
+ current_config_.rot_stopped_vel,current_config_.trans_stopped_vel,
+ current_config_.xy_goal_tolerance,current_config_.yaw_goal_tolerance))
{
ROS_INFO("AckermannPlannerROS: Goal reached");
return true;
@@ -313,7 +284,25 @@ bool AckermannPlannerROS::isGoalReached(void)
else if(this->stuck)
return true;
else
- return false;
+ {
+ double yaw=tf::getYaw(current_pose_.pose.orientation);
+ if(yaw<0.0)
+ yaw+=3.14159*2.0;
+ double goal_dist=sqrt(pow(transformed_plan.back().pose.position.x-current_pose_.pose.position.x,2.0)+pow(transformed_plan.back().pose.position.y-current_pose_.pose.position.y,2.0));
+ double x_dist=((transformed_plan.back().pose.position.x-current_pose_.pose.position.x)*cos(yaw)+(transformed_plan.back().pose.position.y-current_pose_.pose.position.y)*sin(yaw));
+ if(forward && x_dist<0.0 && goal_dist<current_config_.goal_dist)
+ {
+ ROS_INFO_STREAM("AckermannPlannerROS: Goal reached (x_dist: " << x_dist << " goal_dist: " << goal_dist);
+ return true;
+ }
+ else if(!forward && x_dist>0.0 && goal_dist<current_config_.goal_dist)
+ {
+ ROS_INFO_STREAM("AckermannPlannerROS: Goal reached (x_dist: " << x_dist << " goal_dist: " << goal_dist);
+ return true;
+ }
+ else
+ return false;
+ }
}
else
return false;
@@ -350,11 +339,11 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
geometry_msgs::Twist tmp_cmd_vel;
nav_msgs::Odometry odom;
- AckermannPlannerLimits limits;
geometry_msgs::PoseStamped goal_pose;
static int stuck_count=0;
static int replan_count=0;
static bool new_segment=false;
+ bool forward,goal_passed,goal_reached;
// dispatches to either ackermann sampling control or stop and rotate control, depending on whether we have been close enough to goal
if ( ! costmap_ros_->getRobotPose(current_pose_))
@@ -370,7 +359,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
return false;
}
std::vector<geometry_msgs::PoseStamped> transformed_plan;
- if ( ! planner_util_.get_local_plan(current_pose_, transformed_plan))
+ if ( ! planner_util_.get_local_plan(current_pose_, transformed_plan,forward))
{
ROS_ERROR("AckermannPlannerROS: could not get local plan");
cmd_vel.linear.x = 0.0;
@@ -399,18 +388,30 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
ROS_DEBUG_NAMED("ackermann_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size());
// update plan in ackermann_planner even if we just stop and rotate, to allow checkTrajectory
- dp_->update_plan_and_local_costs(current_pose_, transformed_plan);
-
+ dp_->update_plan_and_local_costs(current_pose_, transformed_plan,forward);
+
+ double yaw=tf::getYaw(current_pose_.pose.orientation);
+ if(yaw<0.0)
+ yaw+=3.14159*2.0;
+ if(forward && ((transformed_plan.back().pose.position.x-current_pose_.pose.position.x)*cos(yaw)+(transformed_plan.back().pose.position.y-current_pose_.pose.position.y)*sin(yaw))<0.0)
+ goal_passed=true;
+ else if(!forward && ((transformed_plan.back().pose.position.x-current_pose_.pose.position.x)*cos(yaw)+(transformed_plan.back().pose.position.y-current_pose_.pose.position.y)*sin(yaw))>0.0)
+ goal_passed=true;
+ else
+ goal_passed=false;
odom_helper_.get_odom(odom);
- limits=planner_util_.get_current_limits();
- if(this->stuck || base_local_planner::isGoalReached(*tf_,
- transformed_plan,
- *costmap_ros_->getCostmap(),
- costmap_ros_->getGlobalFrameID(),
- current_pose_,
- odom,
- limits.rot_stopped_vel,limits.trans_stopped_vel,
- limits.xy_goal_tolerance,limits.yaw_goal_tolerance))
+ if(base_local_planner::isGoalReached(*tf_,
+ transformed_plan,
+ *costmap_ros_->getCostmap(),
+ costmap_ros_->getGlobalFrameID(),
+ current_pose_,
+ odom,
+ current_config_.rot_stopped_vel,current_config_.trans_stopped_vel,
+ current_config_.xy_goal_tolerance,current_config_.yaw_goal_tolerance))
+ goal_reached=true;
+ else
+ goal_reached=false;
+ if(this->stuck || goal_reached || goal_passed)
{
if(planner_util_.set_next_path())
{
@@ -453,17 +454,17 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
ackermann_msgs::AckermannDrive ackermann;
odom_helper_.get_ackermann_state(ackermann);
// steer the car without moving if the steering angle is big
- if(this->use_steer_angle_cmd && this->use_stopped_steering)
+ if(current_config_.use_steer_angle_cmd && current_config_.use_stopped_steering)
{
double ang_z = cmd_vel.angular.z;
- if(this->invert_steer_angle_cmd)
+ if(current_config_.invert_steer_angle_cmd)
ang_z = -ang_z;
double diff = fabs(ackermann.steering_angle-ang_z);
- if(diff > this->stopped_steering_angle_threshold)
+ if(diff > current_config_.stopped_steering_angle_threshold)
{
- //ROS_INFO("AckermannPlannerROS: steering without moving")
- //ROS_INFO("AckermannPlannerROS: stopped steering. Angle current - target = %f - %f = %f, vs threshold %f )",ackermann.steering_angle, ang_z, diff, this->stopped_steering_angle_threshold);
+ ROS_INFO("AckermannPlannerROS: steering without moving");
+ ROS_INFO("AckermannPlannerROS: stopped steering. Angle current - target = %f - %f = %f, vs threshold %f )",ackermann.steering_angle, ang_z, diff, current_config_.stopped_steering_angle_threshold);
cmd_vel.linear.x=0.0;
}
else
@@ -482,20 +483,33 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
new_segment=false;
}
}
- else if(this->use_stuck_check && fabs(cmd_vel.linear.x)<this->stuck_check_vel_threshold)
+ else if(current_config_.use_stuck_check && fabs(cmd_vel.linear.x)<current_config_.stuck_check_vel_threshold)
{
- //ROS_INFO_STREAM("linear.x " << cmd_vel.linear.x << " deadzone: " << limits.trans_vel_deadzone );
+ //ROS_INFO_STREAM("linear.x " << cmd_vel.linear.x << " deadzone: " << current_config_.trans_vel_deadzone );
// get the position of the goal
- base_local_planner::getGoalPose(*tf_,
- transformed_plan,
- costmap_ros_->getGlobalFrameID(),
- goal_pose);
+ if(!base_local_planner::getGoalPose(*tf_,
+ transformed_plan,
+ costmap_ros_->getGlobalFrameID(),
+ goal_pose))
+ {
+ ROS_WARN_NAMED("ackermann_local_planner", "Ackermann planner failed to produce path.");
+ std::vector<geometry_msgs::PoseStamped> empty_plan;
+ publish_global_plan(empty_plan);
+ cmd_vel.linear.x = 0.0;
+ cmd_vel.linear.y = 0.0;
+ cmd_vel.angular.z = 0.0;
+ this->stuck=false;
+ new_segment=false;
+ replan_count=0;
+ stuck_count=0;
+ return false;
+ }
// get the distance to the goal
double dist=base_local_planner::getGoalPositionDistance(current_pose_,goal_pose.pose.position.x,goal_pose.pose.position.y);
- if(dist>limits.xy_goal_tolerance && dist<2*limits.xy_goal_tolerance)
+ if(dist>current_config_.xy_goal_tolerance && dist<2*current_config_.xy_goal_tolerance)
{
stuck_count++;
- if(stuck_count>this->stuck_check_max_occurrences)
+ if(stuck_count>current_config_.stuck_check_max_occurrences)
{
this->stuck=true;
ROS_INFO("AckermannPlannerROS: Robot may be stuck near segment end point. Jumping to the next path segment");
@@ -504,7 +518,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
else
{
replan_count++;
- if(replan_count>this->stuck_check_max_occurrences)
+ if(replan_count>current_config_.stuck_check_max_occurrences)
{
ROS_INFO("AckermannPlannerROS: Robot seems stuck. Replanning");
cmd_vel.linear.x = 0.0;
@@ -526,16 +540,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
}
if (isOk)
- {
publish_global_plan(transformed_plan);
- if(limits.use_trans_vel_deadzone && fabs(cmd_vel.linear.x)<limits.trans_vel_deadzone)
- {
- if(cmd_vel.linear.x>0.0)
- cmd_vel.linear.x=limits.trans_vel_deadzone;
- else if(cmd_vel.linear.x<0.0)
- cmd_vel.linear.x=-limits.trans_vel_deadzone;
- }
- }
else
{
ROS_WARN_NAMED("ackermann_local_planner", "Ackermann planner failed to produce path.");
diff --git a/src/ackermann_planner_util.cpp b/src/ackermann_planner_util.cpp
index 11254acddd7b05a3076e7eef7445407690fc01cc..34dc68931e1f3c6c039793a49483fdfc237c8c39 100644
--- a/src/ackermann_planner_util.cpp
+++ b/src/ackermann_planner_util.cpp
@@ -36,6 +36,7 @@
*********************************************************************/
#include <ackermann_planner_util.h>
+#include <tf/transform_datatypes.h>
#include <base_local_planner/goal_functions.h>
@@ -50,6 +51,7 @@ void AckermannPlannerUtil::initialize(tf2_ros::Buffer* tf, costmap_2d::Costmap2D
{
tf_ = tf;
costmap_ = costmap;
+ this->world_model_= new base_local_planner::CostmapModel(*costmap_);
global_frame_ = global_frame;
initialized_ = true;
}
@@ -59,18 +61,19 @@ void AckermannPlannerUtil::initialize(tf2_ros::Buffer* tf, costmap_2d::Costmap2D
}
}
-void AckermannPlannerUtil::reconfigure_callback(AckermannPlannerLimits &config, bool restore_defaults)
+void AckermannPlannerUtil::reconfigure_callback(iri_ackermann_local_planner::AckermannLocalPlannerConfig &config, bool restore_defaults)
{
- if(setup_ && restore_defaults)
- config = default_limits_;
+ static bool setup=false;
- if(!setup_)
+ if(setup && restore_defaults)
+ config = default_config_;
+
+ if(!setup)
{
- default_limits_ = config;
- setup_ = true;
+ default_config_ = config;
+ setup = true;
}
- boost::mutex::scoped_lock l(limits_configuration_mutex_);
- limits_ = AckermannPlannerLimits(config);
+ current_config_=config;
}
costmap_2d::Costmap2D* AckermannPlannerUtil::get_costmap()
@@ -78,10 +81,9 @@ costmap_2d::Costmap2D* AckermannPlannerUtil::get_costmap()
return costmap_;
}
-AckermannPlannerLimits AckermannPlannerUtil::get_current_limits()
+base_local_planner::CostmapModel *AckermannPlannerUtil::get_world_model()
{
- boost::mutex::scoped_lock l(limits_configuration_mutex_);
- return limits_;
+ return world_model_;
}
std::string AckermannPlannerUtil::get_global_frame(void)
@@ -101,57 +103,97 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
ROS_ERROR("AckermannPlannerUtil: planner utils have not been initialized, please call initialize() first");
return false;
}
- ////divide plan by manuveurs
subPathList.clear();
subPath.clear();
+ forward_segment.clear();
subPathIndex = 0;
- double pathLength = 0;
+ double pathLength = 0, yaw;
for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
+ std::cout << orig_global_plan[i].pose.position.x << "," << orig_global_plan[i].pose.position.y << "," << tf::getYaw(orig_global_plan[i].pose.orientation) << std::endl;
+ if(orig_global_plan.size()<5)
{
- if(i>2 && i<(orig_global_plan.size()-1))
+ for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
+ subPath.push_back(orig_global_plan[i]);
+ }
+ else
+ {
+ ////divide plan by manuveurs
+ for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
{
- double x_1 = orig_global_plan[i+1].pose.position.x;
- double x0 = orig_global_plan[i ].pose.position.x;
- double x1 = orig_global_plan[i-1].pose.position.x;
- double x2 = orig_global_plan[i-2].pose.position.x;
- double x3 = orig_global_plan[i-3].pose.position.x;
- double y0 = orig_global_plan[i ].pose.position.y;
- double y_1 = orig_global_plan[i+1].pose.position.y;
- double y1 = orig_global_plan[i-1].pose.position.y;
- double y2 = orig_global_plan[i-2].pose.position.y;
- double y3 = orig_global_plan[i-3].pose.position.y;
- double angle=((x0-x1)*(x1-x2)+(y0-y1)*(y1-y2))/(std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2))*std::sqrt(std::pow(x1-x2,2)+std::pow(y1-y2,2)));
- if(angle<-1.0)
- angle=-1.0;
- else if(angle>1.0)
- angle=1.0;
- angle=std::acos(angle);
- pathLength+= std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2));
- if(fabs(angle)>1.57) //if changes of direction detected
+ if(i>2 && i<(orig_global_plan.size()-1))
{
- angle=((x_1-x0)*(x2-x3)+(y_1-y0)*(y2-y3))/(std::sqrt(std::pow(x_1-x0,2)+std::pow(y_1-y0,2))*std::sqrt(std::pow(x2-x3,2)+std::pow(y2-y3,2)));
+ double x_1 = orig_global_plan[i+1].pose.position.x;
+ double x0 = orig_global_plan[i ].pose.position.x;
+ double x1 = orig_global_plan[i-1].pose.position.x;
+ double x2 = orig_global_plan[i-2].pose.position.x;
+ double x3 = orig_global_plan[i-3].pose.position.x;
+ double y0 = orig_global_plan[i ].pose.position.y;
+ double y_1 = orig_global_plan[i+1].pose.position.y;
+ double y1 = orig_global_plan[i-1].pose.position.y;
+ double y2 = orig_global_plan[i-2].pose.position.y;
+ double y3 = orig_global_plan[i-3].pose.position.y;
+ double angle=((x0-x1)*(x1-x2)+(y0-y1)*(y1-y2))/(std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2))*std::sqrt(std::pow(x1-x2,2)+std::pow(y1-y2,2)));
+ double curvature;
+ curvature=((x_1-x0)*(y0-2*y1+y2)-(x0-2*x1+x2)*(y_1-y0))/sqrt(pow(pow(x_1-x0,2.0)+pow(y_1-y0,2.0),3.0));
+// std::cout << curvature << std::endl;
if(angle<-1.0)
angle=-1.0;
else if(angle>1.0)
angle=1.0;
angle=std::acos(angle);
- if(fabs(angle)>1.57)
+ pathLength+= std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2));
+ if(fabs(angle)>1.57) //if changes of direction detected
{
- if(pathLength>limits_.split_ignore_length)
- {
- //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, length=%f", i, angle, pathLength);
- subPathList.push_back(subPath);
- }
- else //ignored subpath, too short
+ angle=((x_1-x0)*(x2-x3)+(y_1-y0)*(y2-y3))/(std::sqrt(std::pow(x_1-x0,2)+std::pow(y_1-y0,2))*std::sqrt(std::pow(x2-x3,2)+std::pow(y2-y3,2)));
+ if(angle<-1.0)
+ angle=-1.0;
+ else if(angle>1.0)
+ angle=1.0;
+ angle=std::acos(angle);
+ if(fabs(angle)>1.57)
{
- //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, Ignored by length=%f", i, angle, pathLength);
+ if(pathLength>current_config_.split_ignore_length)
+ {
+ //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, length=%f", i, angle, pathLength);
+ yaw=tf::getYaw(subPath[0].pose.orientation);
+ if(yaw<0.0)
+ yaw+=3.14159*2.0;
+ if(((subPath[subPath.size()-1].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[subPath.size()-1].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
+ {
+ std::cout << "backward segment" << std::endl;
+ this->forward_segment.push_back(false);
+ }
+ else
+ {
+ std::cout << "forward segment" << std::endl;
+ this->forward_segment.push_back(true);
+ }
+ subPathList.push_back(subPath);
+ }
+ else //ignored subpath, too short
+ {
+ //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, Ignored by length=%f", i, angle, pathLength);
+ }
+ subPath.clear();
+ pathLength=0.0;
}
- subPath.clear();
- pathLength=0.0;
}
}
+ subPath.push_back(orig_global_plan[i]);
}
- subPath.push_back(orig_global_plan[i]);
+ }
+ yaw=tf::getYaw(subPath[0].pose.orientation);
+ if(yaw<0.0)
+ yaw+=3.14159*2.0;
+ if(((subPath[subPath.size()-1].pose.position.x-subPath[0].pose.position.x)*cos(yaw)+(subPath[subPath.size()-1].pose.position.y-subPath[0].pose.position.y)*sin(yaw))<0.0)
+ {
+ std::cout << "backward segment" << std::endl;
+ this->forward_segment.push_back(false);
+ }
+ else
+ {
+ std::cout << "forward segment" << std::endl;
+ this->forward_segment.push_back(true);
}
subPathList.push_back(subPath);
//ROS_INFO("AckermannPlannerUtil::setPlan: subPath last length=%f", pathLength);
@@ -161,6 +203,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
global_plan_.clear();
global_plan_ = subPathList[subPathIndex];
+ this->plan_forward=this->forward_segment[subPathIndex];
return true;
}
@@ -173,6 +216,7 @@ bool AckermannPlannerUtil::set_next_path(void)
subPathIndex++;
global_plan_.clear();
global_plan_ = subPathList[subPathIndex];
+ this->plan_forward=this->forward_segment[subPathIndex];
return true;
}
else
@@ -188,10 +232,13 @@ bool AckermannPlannerUtil::last_path(void)
return false;
}
-bool AckermannPlannerUtil::get_local_plan(geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan)
+bool AckermannPlannerUtil::get_local_plan(geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan,bool &forward)
{
- global_pose.header.stamp-=ros::Duration(1.0);
-
+ if (!tf_->canTransform(global_frame_, ros::Time::now(), global_plan_[0].header.frame_id, global_plan_[0].header.stamp, global_plan_[0].header.frame_id, ros::Duration(0.5)))
+ {
+ ROS_ERROR_STREAM("AckermannPlannerUtil::get_local_plan -> no tf from " << global_frame_ << " to " << global_plan_[0].header.frame_id << " from " << global_plan_[0].header.stamp << " to " << ros::Time::now());
+ return false;
+ }
//get the global plan in our frame
if(!base_local_planner::transformGlobalPlan(*tf_,global_plan_,global_pose,*costmap_,global_frame_,transformed_plan)) {
ROS_WARN("AckermannPlannerUtil: could not transform the global plan to the frame of the controller");
@@ -199,13 +246,19 @@ bool AckermannPlannerUtil::get_local_plan(geometry_msgs::PoseStamped& global_pos
}
//now we'll prune the plan based on the position of the robot
- if(limits_.prune_plan)
+ if(current_config_.prune_plan)
base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
+ forward=plan_forward;
+
return true;
}
AckermannPlannerUtil::~AckermannPlannerUtil()
{
-
+ if(this->world_model_!=NULL)
+ {
+ delete this->world_model_;
+ this->world_model_=NULL;
+ }
}
diff --git a/src/ackermann_spline_generator.cpp b/src/ackermann_spline_generator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a88edb528b9a251567a8317bbf5d933160dde50
--- /dev/null
+++ b/src/ackermann_spline_generator.cpp
@@ -0,0 +1,286 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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 OWNER 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.
+ *
+ * Author: TKruse
+ *********************************************************************/
+
+#include <ackermann_spline_generator.h>
+
+#include <cmath>
+#include <iostream>
+#include <algorithm>
+#include <limits>
+
+#include <ros/console.h>
+#include <tf/transform_datatypes.h>
+
+#include "exceptions.h"
+
+AckermannSplineGenerator::AckermannSplineGenerator()
+{
+}
+
+unsigned int AckermannSplineGenerator::get_path_closest_pose(double &path_x,double &path_y)
+{
+ double dist,min_dist=std::numeric_limits<double>::max();
+ unsigned int min_index=0;
+
+ /* find the path point closest to the robot */
+ for(unsigned int i=0;i<this->current_plan.size();i++)
+ {
+ dist=sqrt(pow(pos_(0)-this->current_plan[i].pose.position.x,2.0)+pow(pos_(1)-this->current_plan[i].pose.position.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ min_index=i;
+ }
+ }
+ path_x=this->current_plan[min_index].pose.position.x;
+ path_y=this->current_plan[min_index].pose.position.y;
+
+ return min_index;
+}
+
+void AckermannSplineGenerator::initialise(
+ const Eigen::Vector3f& pos,
+ const Eigen::Vector3f& ackermann,//[0] -> trans_vel, [1]-> steer_angle, [2]-> steer_vel
+ const Eigen::Vector3f& goal,
+ bool forward,
+ std::vector<geometry_msgs::PoseStamped>& current_plan)
+{
+ double diff_x,diff_y,current_x,current_y,step;
+ double min_n1,max_n1,min_n2,max_n2,min_n3,max_n3,min_n4,max_n4;
+ double n1_step, n2_step, n3_step, n4_step;
+ unsigned int int_step,current_index,num_samples;
+ TSplineSample new_sample;
+
+ try{
+ this->forward=forward;
+ this->current_plan=current_plan;
+ pos_=pos;
+ ackermann_=ackermann;
+ goal_pos_=goal;
+ next_sample_index_=0;
+ min_n1=0.0;
+ min_n2=0.0;
+ min_n3=0.0;
+ max_n3=0.0;
+ min_n4=0.0;
+ max_n4=0.0;
+ samples_.clear();
+ current_index=this->get_path_closest_pose(current_x,current_y);
+ step=(current_plan.size()-current_index)/current_config_.dist_samples;
+ if(step<current_config_.min_segment_points)
+ {
+ num_samples=(current_plan.size()-current_index)/current_config_.min_segment_points;
+ if(num_samples==0)
+ {
+ num_samples=1;
+ step=(current_plan.size()-current_index);
+ }
+ else
+ step=(current_plan.size()-current_index)/num_samples;
+ }
+ else
+ num_samples=current_config_.dist_samples;
+ for(unsigned int i=0;i<num_samples;i++)
+ {
+ int_step=current_index+(unsigned int)(step*(i+1));
+ Eigen::Vector3f tmp_goal=Eigen::Vector3f(current_plan[int_step-1].pose.position.x,current_plan[int_step-1].pose.position.y,tf::getYaw(current_plan[int_step-1].pose.orientation));
+ diff_x=sqrt(pow(tmp_goal(0)-pos(0),2));
+ diff_y=sqrt(pow(tmp_goal(1)-pos(1),2));
+ max_n1=2.0*diff_x;
+ if (max_n1 < min_n1)
+ max_n1 = min_n1;
+ max_n2 = 2.0*diff_y;
+ if (max_n2 < min_n2)
+ max_n2 = min_n2;
+
+ if (current_config_.n1_samples == 1 || max_n1 == min_n1)
+ n1_step = std::numeric_limits<double>::max();
+ else
+ n1_step = (max_n1-min_n1)/(current_config_.n1_samples-1);
+ if (current_config_.n2_samples == 1 || max_n2 == min_n2)
+ n2_step = std::numeric_limits<double>::max();
+ else
+ n2_step = (max_n2-min_n2)/(current_config_.n2_samples-1);
+ if (current_config_.n3_samples == 1 || max_n3 == min_n3)
+ n3_step = std::numeric_limits<double>::max();
+ else
+ n3_step = (max_n3-min_n3)/(current_config_.n3_samples-1);
+ if (current_config_.n4_samples == 1 || max_n4 == min_n4)
+ n4_step = std::numeric_limits<double>::max();
+ else
+ n4_step = (max_n4-min_n4)/(current_config_.n4_samples-1);
+
+ for(double current_n1=min_n1;current_n1<=max_n1;current_n1+=n1_step)
+ for(double current_n2=min_n2;current_n2<=max_n2;current_n2+=n2_step)
+ for(double current_n3=min_n3;current_n3<=max_n3;current_n3+=n3_step)
+ for(double current_n4=min_n4;current_n4<=max_n4;current_n4+=n4_step)
+ {
+ new_sample.params=Eigen::Vector4f(current_n1,current_n2,current_n3,current_n4);
+ new_sample.end_point.x=tmp_goal(0);
+ new_sample.end_point.y=tmp_goal(1);
+ new_sample.end_point.heading=tmp_goal(2);
+ samples_.push_back(new_sample);
+ }
+ }
+ }catch(std::exception &e){
+ std::cout << e.what() << std::endl;
+ }
+
+ //Calculate lateral offset
+ y_offset=(-(current_x-pos_(0))*sin(pos_(2))+(current_y-pos_(1))*cos(pos_(2)));
+
+}
+
+void AckermannSplineGenerator::reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg)
+{
+ current_config_=cfg;
+}
+
+/**
+ * Whether this generator can create more trajectories
+ */
+bool AckermannSplineGenerator::hasMoreTrajectories()
+{
+ return next_sample_index_ < samples_.size();
+}
+
+/**
+ * Create and return the next sample trajectory
+ */
+bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &comp_traj)
+{
+ TPoint start,end;
+ double resolution,k_max;
+ std::vector<double> x,y,curvature,heading;
+
+ try{
+ start.x=pos_(0);
+ start.y=pos_(1);
+ if(this->forward)
+ start.heading=pos_(2);
+ else
+ start.heading=pos_(2)+3.14159;
+ if(fabs(ackermann_(1))<0.001)
+ start.curvature=0.0;
+ else
+ start.curvature=tan(ackermann_(1))/current_config_.axis_distance;
+ spline_.set_start_point(start);
+ //std::cout << "start position: " << pos_(0) << "," << pos_(1) << "," << pos_(2) << "," << start.curvature << std::endl;
+ end.x=samples_[next_sample_index_].end_point.x;
+ end.y=samples_[next_sample_index_].end_point.y;
+ if(this->forward)
+ end.heading=samples_[next_sample_index_].end_point.heading;
+ else
+ end.heading=samples_[next_sample_index_].end_point.heading+3.14159;
+ end.curvature=0.0;
+ spline_.set_end_point(end);
+ //std::cout << "end position: " << end.x << "," << end.y << "," << end.heading << "," << end.curvature << std::endl;
+ resolution=current_config_.sim_granularity;
+ spline_.generate(resolution,samples_[next_sample_index_].params(0),samples_[next_sample_index_].params(1),samples_[next_sample_index_].params(2),samples_[next_sample_index_].params(3));
+ spline_.evaluate_all(x,y,curvature,heading);
+ // std::cout << "generate: " << samples_[next_sample_index_].params(0) << "," << samples_[next_sample_index_].params(1) << "," << samples_[next_sample_index_].params(2) << "," << samples_[next_sample_index_].params(3) << std::endl;
+ // discard trajectories with two much curvature
+ k_max=0.0;
+ for(unsigned int i=0;i<curvature.size();i++)
+ if(fabs(curvature[i])>k_max)
+ k_max=fabs(curvature[i]);
+ next_sample_index_++;
+ if(k_max>current_config_.max_curvature)
+ return false;
+ /* fill the ouput trajectory object */
+ double k_max_speed,min_max_speed,new_start_vel,new_max_vel;
+ double acceleration,length,steer_angle;
+ CVelTrapezoid vel_profile;
+ TPoint point;
+
+ if(k_max==0)
+ k_max_speed=std::numeric_limits<double>::max();
+ else
+ k_max_speed=sqrt(current_config_.comfort_lat_acc/fabs(k_max));
+ min_max_speed=std::min(current_config_.max_trans_vel,k_max_speed);
+ if(vel_profile.generate(fabs(ackermann_(0)),0.0,min_max_speed,current_config_.max_trans_acc,spline_.get_length(),new_start_vel,new_max_vel))
+ {
+ try{
+ vel_profile.evaluate_at_time(current_config_.temp_horizon,comp_traj.xv_,acceleration,length);
+ }catch(CException &e){
+ length=spline_.get_length();
+ comp_traj.xv_=vel_profile.get_max_vel();
+ }
+ if(!this->forward)
+ comp_traj.xv_*=-1.0;
+ comp_traj.yv_=0.0;
+ spline_.evaluate(length, point);
+ comp_traj.thetav_=-(point.heading-start.heading);
+ if(comp_traj.thetav_>3.14159)
+ comp_traj.thetav_-=2*3.14159;
+ else if(comp_traj.thetav_<-3.14159)
+ comp_traj.thetav_+=2*3.14159;
+ if(forward)
+ comp_traj.thetav_-=atan2(current_config_.lateral_offset_gain*y_offset,comp_traj.xv_);
+ else
+ comp_traj.thetav_+=atan2(current_config_.lateral_offset_gain*y_offset,-comp_traj.xv_);
+ if(forward)
+ comp_traj.thetav_*=-1.0;
+ if(comp_traj.thetav_>current_config_.max_steer_angle)
+ comp_traj.thetav_=current_config_.max_steer_angle;
+ else if(comp_traj.thetav_<current_config_.min_steer_angle)
+ comp_traj.thetav_=current_config_.min_steer_angle;
+ comp_traj.resetPoints();
+ for(unsigned int i=0;i<x.size();i++)
+ comp_traj.addPoint(x[i],y[i],heading[i]);
+ return true;
+ }
+ else
+ return false;
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ return false;
+ }catch(std::exception &e){
+ std::cout << e.what() << std::endl;
+ return false;
+ }
+}
+
+void AckermannSplineGenerator::set_sim_period(double period)
+{
+ sim_period_=period;
+}
+
+AckermannSplineGenerator::~AckermannSplineGenerator()
+{
+
+}
diff --git a/src/ackermann_trajectory_generator.cpp b/src/ackermann_trajectory_generator.cpp
index 789c1bec6b67391fa0c1da31ac61dc6e83f0d332..85a77ca0ceddc185a4997864b8e0bdae3d1edb8c 100644
--- a/src/ackermann_trajectory_generator.cpp
+++ b/src/ackermann_trajectory_generator.cpp
@@ -47,16 +47,12 @@
AckermannTrajectoryGenerator::AckermannTrajectoryGenerator()
{
- limits_=NULL;
}
void AckermannTrajectoryGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& ackermann,//[0] -> trans_vel, [1]-> steer_angle, [2]-> steer_vel
- const Eigen::Vector3f& goal,
- AckermannPlannerLimits* limits,
- const Eigen::Vector2f& vsamples,//trans_vel samples & steer_angle samples
- bool discretize_by_time)
+ const Eigen::Vector3f& goal)
{
double steer_angle,steer_vel,trans_vel;
double max_steer_vel,min_steer_vel;
@@ -67,196 +63,206 @@ void AckermannTrajectoryGenerator::initialise(
* We actually generate all velocity sample vectors here, from which to generate trajectories later on
*/
pos_ = pos;
- limits_ = limits;
ackermann_ = ackermann;
next_sample_index_ = 0;
sample_params_.clear();
// saturate the current ackermann state if necessary
-/* if(ackermann[0]>limits->max_trans_vel)
- trans_vel=limits->max_trans_vel;
- else if (ackermann[0]<limits->min_trans_vel)
- trans_vel=limits->min_trans_vel;
+/* if(ackermann[0]>current_config_.max_trans_vel)
+ trans_vel=current_config_.max_trans_vel;
+ else if (ackermann[0]<current_config_.min_trans_vel)
+ trans_vel=current_config_.min_trans_vel;
else*/
trans_vel=ackermann[0];
-/* if(ackermann[1]>limits->max_steer_angle)
- steer_angle=limits->max_steer_angle;
- else if(ackermann[1]<limits->min_steer_angle)
- steer_angle=limits->min_steer_angle;
+/* if(ackermann[1]>current_config_.max_steer_angle)
+ steer_angle=current_config_.max_steer_angle;
+ else if(ackermann[1]<current_config_.min_steer_angle)
+ steer_angle=current_config_.min_steer_angle;
else */
steer_angle=ackermann[1];
-/* if(ackermann[2]>limits->max_steer_vel)
- steer_vel=limits->max_steer_vel;
- else if(ackermann[2]<limits->min_steer_vel)
- steer_vel=limits->min_steer_vel;
+/* if(ackermann[2]>current_config_.max_steer_vel)
+ steer_vel=current_config_.max_steer_vel;
+ else if(ackermann[2]<current_config_.min_steer_vel)
+ steer_vel=current_config_.min_steer_vel;
else*/
steer_vel=ackermann[2];
// compute the simulation time
double dist=sqrt((goal[0]-pos[0])*(goal[0]-pos[0])+(goal[1]-pos[1])*(goal[1]-pos[1]));
- sim_time_=dist/limits->max_trans_vel+limits->max_trans_vel/limits->max_trans_acc;
- if(sim_time_>max_sim_time_)
- sim_time_=max_sim_time_;
- else if(sim_time_<min_sim_time_)
- sim_time_=min_sim_time_;
+ sim_time_=dist/current_config_.max_trans_vel+current_config_.max_trans_vel/current_config_.max_trans_acc;
+ if(sim_time_>current_config_.max_sim_time)
+ sim_time_=current_config_.max_sim_time;
+ else if(sim_time_<current_config_.min_sim_time)
+ sim_time_=current_config_.min_sim_time;
// compute the trajectory times and windows
if(steer_vel>=0)
{
- max_steer_vel=(steer_vel+limits->max_steer_acc*sim_time_)/2.0;
- if(max_steer_vel>=limits->max_steer_vel)
+ max_steer_vel=(steer_vel+current_config_.max_steer_acc*sim_time_)/2.0;
+ if(max_steer_vel>=current_config_.max_steer_vel)
{
- max_steer_vel=limits->max_steer_vel;
- T4=sim_time_-(2.0*max_steer_vel-steer_vel)/limits->max_steer_acc;
- max_steer_angle=max_steer_vel*max_steer_vel/limits->max_steer_acc-steer_vel*steer_vel/(2.0*limits->max_steer_acc)+max_steer_vel*T4+steer_angle;
- if(max_steer_angle>limits->max_steer_angle)
+ max_steer_vel=current_config_.max_steer_vel;
+ T4=sim_time_-(2.0*max_steer_vel-steer_vel)/current_config_.max_steer_acc;
+ max_steer_angle=max_steer_vel*max_steer_vel/current_config_.max_steer_acc-steer_vel*steer_vel/(2.0*current_config_.max_steer_acc)+max_steer_vel*T4+steer_angle;
+ if(max_steer_angle>current_config_.max_steer_angle)
{
- max_steer_angle=limits->max_steer_angle;
- T4=(limits->max_steer_angle-steer_angle-max_steer_vel*max_steer_vel/limits->max_steer_acc+steer_vel*steer_vel/(2.0*limits->max_steer_acc))/max_steer_vel;
+ max_steer_angle=current_config_.max_steer_angle;
+ T4=(current_config_.max_steer_angle-steer_angle-max_steer_vel*max_steer_vel/current_config_.max_steer_acc+steer_vel*steer_vel/(2.0*current_config_.max_steer_acc))/max_steer_vel;
if(T4<0)
{
T4=0;
- max_steer_vel=sqrt(steer_vel*steer_vel/2.0+limits->max_steer_acc*(limits->max_steer_angle-steer_angle));
+ max_steer_vel=sqrt(steer_vel*steer_vel/2.0+current_config_.max_steer_acc*(current_config_.max_steer_angle-steer_angle));
}
}
}
else
{
- max_steer_angle=max_steer_vel*max_steer_vel/limits->max_steer_acc-steer_vel*steer_vel/(2.0*limits->max_steer_acc)+steer_angle;
+ max_steer_angle=max_steer_vel*max_steer_vel/current_config_.max_steer_acc-steer_vel*steer_vel/(2.0*current_config_.max_steer_acc)+steer_angle;
T4=0;
- if(max_steer_angle>limits->max_steer_angle)
+ if(max_steer_angle>current_config_.max_steer_angle)
{
- max_steer_angle=limits->max_steer_angle;
- max_steer_vel=sqrt(steer_vel*steer_vel/2.0+limits->max_steer_acc*(limits->max_steer_angle-steer_angle));
+ max_steer_angle=current_config_.max_steer_angle;
+ max_steer_vel=sqrt(steer_vel*steer_vel/2.0+current_config_.max_steer_acc*(current_config_.max_steer_angle-steer_angle));
}
}
- min_steer_vel=(steer_vel-limits->max_steer_acc*sim_time_)/2.0;
- if(min_steer_vel<=limits->min_steer_vel)
+ min_steer_vel=(steer_vel-current_config_.max_steer_acc*sim_time_)/2.0;
+ if(min_steer_vel<=current_config_.min_steer_vel)
{
- min_steer_vel=limits->min_steer_vel;
- T4=sim_time_+(2.0*min_steer_vel-steer_vel)/limits->max_steer_acc;
- min_steer_angle=steer_vel*steer_vel/(2.0*limits->max_steer_acc)-min_steer_vel*min_steer_vel/limits->max_steer_acc+min_steer_vel*T4+steer_angle;
- if(min_steer_angle<limits->min_steer_angle)
+ min_steer_vel=current_config_.min_steer_vel;
+ T4=sim_time_+(2.0*min_steer_vel-steer_vel)/current_config_.max_steer_acc;
+ min_steer_angle=steer_vel*steer_vel/(2.0*current_config_.max_steer_acc)-min_steer_vel*min_steer_vel/current_config_.max_steer_acc+min_steer_vel*T4+steer_angle;
+ if(min_steer_angle<current_config_.min_steer_angle)
{
- min_steer_angle=limits->min_steer_angle;
- T4=(limits->min_steer_angle-steer_angle+min_steer_vel*min_steer_vel/limits->max_steer_acc-steer_vel*steer_vel/(2.0*limits->max_steer_acc))/min_steer_vel;
+ min_steer_angle=current_config_.min_steer_angle;
+ T4=(current_config_.min_steer_angle-steer_angle+min_steer_vel*min_steer_vel/current_config_.max_steer_acc-steer_vel*steer_vel/(2.0*current_config_.max_steer_acc))/min_steer_vel;
if(T4<0)
{
T4=0;
- min_steer_vel=-sqrt(steer_vel*steer_vel/2.0-limits->max_steer_acc*(limits->min_steer_angle-steer_angle));
+ min_steer_vel=-sqrt(steer_vel*steer_vel/2.0-current_config_.max_steer_acc*(current_config_.min_steer_angle-steer_angle));
}
}
}
else
{
- min_steer_angle=steer_vel*steer_vel/(2.0*limits->max_steer_acc)-min_steer_vel*min_steer_vel/limits->max_steer_acc+steer_angle;
+ min_steer_angle=steer_vel*steer_vel/(2.0*current_config_.max_steer_acc)-min_steer_vel*min_steer_vel/current_config_.max_steer_acc+steer_angle;
T4=0;
- if(min_steer_angle<limits->min_steer_angle)
+ if(min_steer_angle<current_config_.min_steer_angle)
{
- min_steer_angle=limits->min_steer_angle;
- min_steer_vel=-sqrt(steer_vel*steer_vel/2.0-limits->max_steer_acc*(limits->min_steer_angle-steer_angle));
+ min_steer_angle=current_config_.min_steer_angle;
+ min_steer_vel=-sqrt(steer_vel*steer_vel/2.0-current_config_.max_steer_acc*(current_config_.min_steer_angle-steer_angle));
}
}
}
else
{
- max_steer_vel=(steer_vel+limits->max_steer_acc*sim_time_)/2.0;
- if(max_steer_vel>=limits->max_steer_vel)
+ max_steer_vel=(steer_vel+current_config_.max_steer_acc*sim_time_)/2.0;
+ if(max_steer_vel>=current_config_.max_steer_vel)
{
- max_steer_vel=limits->max_steer_vel;
- T4=sim_time_-(2.0*max_steer_vel-steer_vel)/limits->max_steer_acc;
- max_steer_angle=-steer_vel*steer_vel/(2.0*limits->max_steer_acc)+max_steer_vel*max_steer_vel/limits->max_steer_acc+max_steer_vel*T4+steer_angle;
- if(max_steer_angle>limits->max_steer_angle)
+ max_steer_vel=current_config_.max_steer_vel;
+ T4=sim_time_-(2.0*max_steer_vel-steer_vel)/current_config_.max_steer_acc;
+ max_steer_angle=-steer_vel*steer_vel/(2.0*current_config_.max_steer_acc)+max_steer_vel*max_steer_vel/current_config_.max_steer_acc+max_steer_vel*T4+steer_angle;
+ if(max_steer_angle>current_config_.max_steer_angle)
{
- max_steer_angle=limits->max_steer_angle;
- T4=(limits->max_steer_angle-steer_angle-max_steer_vel*max_steer_vel/limits->max_steer_acc+steer_vel*steer_vel/(2.0*limits->max_steer_acc))/max_steer_vel;
+ max_steer_angle=current_config_.max_steer_angle;
+ T4=(current_config_.max_steer_angle-steer_angle-max_steer_vel*max_steer_vel/current_config_.max_steer_acc+steer_vel*steer_vel/(2.0*current_config_.max_steer_acc))/max_steer_vel;
if(T4<0)
{
T4=0;
- max_steer_vel=sqrt(steer_vel*steer_vel/2.0+limits->max_steer_acc*(limits->max_steer_angle-steer_angle));
+ max_steer_vel=sqrt(steer_vel*steer_vel/2.0+current_config_.max_steer_acc*(current_config_.max_steer_angle-steer_angle));
}
}
}
else
{
- max_steer_angle=-steer_vel*steer_vel/(2.0*limits->max_steer_acc)+max_steer_vel*max_steer_vel/limits->max_steer_acc+steer_angle;
+ max_steer_angle=-steer_vel*steer_vel/(2.0*current_config_.max_steer_acc)+max_steer_vel*max_steer_vel/current_config_.max_steer_acc+steer_angle;
T4=0;
- if(max_steer_angle>limits->max_steer_angle)
+ if(max_steer_angle>current_config_.max_steer_angle)
{
- max_steer_angle=limits->max_steer_angle;
- max_steer_vel=sqrt(steer_vel*steer_vel/2.0+limits->max_steer_acc*(limits->max_steer_angle-steer_angle));
+ max_steer_angle=current_config_.max_steer_angle;
+ max_steer_vel=sqrt(steer_vel*steer_vel/2.0+current_config_.max_steer_acc*(current_config_.max_steer_angle-steer_angle));
}
}
- min_steer_vel=(steer_vel-limits->max_steer_acc*sim_time_)/2.0;
- if(min_steer_vel<=limits->min_steer_vel)
+ min_steer_vel=(steer_vel-current_config_.max_steer_acc*sim_time_)/2.0;
+ if(min_steer_vel<=current_config_.min_steer_vel)
{
- min_steer_vel=limits->min_steer_vel;
- T4=sim_time_+(2.0*min_steer_vel-steer_vel)/limits->max_steer_acc;
- min_steer_angle=-min_steer_vel*min_steer_vel/limits->max_steer_acc+steer_vel*steer_vel/(2.0*limits->max_steer_acc)+min_steer_vel*T4+steer_angle;
- if(min_steer_angle<limits->min_steer_angle)
+ min_steer_vel=current_config_.min_steer_vel;
+ T4=sim_time_+(2.0*min_steer_vel-steer_vel)/current_config_.max_steer_acc;
+ min_steer_angle=-min_steer_vel*min_steer_vel/current_config_.max_steer_acc+steer_vel*steer_vel/(2.0*current_config_.max_steer_acc)+min_steer_vel*T4+steer_angle;
+ if(min_steer_angle<current_config_.min_steer_angle)
{
- min_steer_angle=limits->min_steer_angle;
- T4=(limits->min_steer_angle-steer_angle+min_steer_vel*min_steer_vel/limits->max_steer_acc-steer_vel*steer_vel/(2.0*limits->max_steer_acc))/min_steer_vel;
+ min_steer_angle=current_config_.min_steer_angle;
+ T4=(current_config_.min_steer_angle-steer_angle+min_steer_vel*min_steer_vel/current_config_.max_steer_acc-steer_vel*steer_vel/(2.0*current_config_.max_steer_acc))/min_steer_vel;
if(T4<0)
{
T4=0;
- min_steer_vel=-sqrt(steer_vel*steer_vel/2.0-limits->max_steer_acc*(limits->min_steer_angle-steer_angle));
+ min_steer_vel=-sqrt(steer_vel*steer_vel/2.0-current_config_.max_steer_acc*(current_config_.min_steer_angle-steer_angle));
}
}
}
else
{
- min_steer_angle=-min_steer_vel*min_steer_vel/limits->max_steer_acc+steer_vel*steer_vel/(2.0*limits->max_steer_acc)+steer_angle;
+ min_steer_angle=-min_steer_vel*min_steer_vel/current_config_.max_steer_acc+steer_vel*steer_vel/(2.0*current_config_.max_steer_acc)+steer_angle;
T4=0;
- if(min_steer_angle<limits->min_steer_angle)
+ if(min_steer_angle<current_config_.min_steer_angle)
{
- min_steer_angle=limits->min_steer_angle;
- min_steer_vel=-sqrt(steer_vel*steer_vel/2.0-limits->max_steer_acc*(limits->min_steer_angle-steer_angle));
+ min_steer_angle=current_config_.min_steer_angle;
+ min_steer_vel=-sqrt(steer_vel*steer_vel/2.0-current_config_.max_steer_acc*(current_config_.min_steer_angle-steer_angle));
}
}
}
double Tacc,Tdeacc;
// maximum translation speed
- Tacc=(limits->max_trans_vel-trans_vel)/limits->max_trans_acc;
- Tdeacc=limits->max_trans_vel/limits->max_trans_acc;
+ Tacc=(current_config_.max_trans_vel-trans_vel)/current_config_.max_trans_acc;
+ Tdeacc=current_config_.max_trans_vel/current_config_.max_trans_acc;
if((sim_time_-Tacc-Tdeacc)>0.0)
- max_trans_vel=limits->max_trans_vel;
+ max_trans_vel=current_config_.max_trans_vel;
else
- max_trans_vel=(sim_time_*limits->max_trans_acc+trans_vel)/2.0;
+ max_trans_vel=(sim_time_*current_config_.max_trans_acc+trans_vel)/2.0;
// minimum translation speed
- Tacc=-(limits->min_trans_vel-trans_vel)/limits->max_trans_acc;
- Tdeacc=-(limits->min_trans_vel)/limits->max_trans_acc;
+ Tacc=-(current_config_.min_trans_vel-trans_vel)/current_config_.max_trans_acc;
+ Tdeacc=-(current_config_.min_trans_vel)/current_config_.max_trans_acc;
if((sim_time_-Tacc-Tdeacc)>0.0)
- min_trans_vel=limits->min_trans_vel;
+ min_trans_vel=current_config_.min_trans_vel;
else
- min_trans_vel=-(sim_time_*limits->max_trans_acc-trans_vel)/2.0;
+ min_trans_vel=-(sim_time_*current_config_.max_trans_acc-trans_vel)/2.0;
/* compute the margins */
Eigen::Vector2f sample = Eigen::Vector2f::Zero();
- base_local_planner::VelocityIterator trans_vel_it(min_trans_vel, max_trans_vel, vsamples[0]);
- base_local_planner::VelocityIterator steer_angle_it(min_steer_angle, max_steer_angle, vsamples[1]);
+ int trans_vel_samp, steer_angle_angular_vel_samp;
+ trans_vel_samp = current_config_.trans_vel_samples;
+ if(current_config_.use_steer_angle_cmd)
+ steer_angle_angular_vel_samp = current_config_.steer_angle_samples;
+ else
+ steer_angle_angular_vel_samp = current_config_.angular_vel_samples;
+
+ if (trans_vel_samp <= 0)
+ {
+ ROS_WARN("You've specified that you don't want any samples in the translational dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
+ trans_vel_samp = 1;
+ }
+
+ if (steer_angle_angular_vel_samp <= 0)
+ {
+ ROS_WARN("You've specified that you don't want any samples in the steering/angular velocity dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead");
+ steer_angle_angular_vel_samp = 1;
+ }
+
+ base_local_planner::VelocityIterator trans_vel_it(min_trans_vel, max_trans_vel, trans_vel_samp);
+ base_local_planner::VelocityIterator steer_angle_it(min_steer_angle, max_steer_angle, steer_angle_angular_vel_samp);
for(; !trans_vel_it.isFinished(); trans_vel_it++)
{
- sample[0] = trans_vel_it.getVelocity();
- for(; !steer_angle_it.isFinished(); steer_angle_it++)
+ if(!current_config_.use_trans_vel_deadzone || (current_config_.use_trans_vel_deadzone && trans_vel_it.getVelocity()>current_config_.trans_vel_deadzone))
{
- sample[1] = steer_angle_it.getVelocity();
- sample_params_.push_back(sample);
+ sample[0] = trans_vel_it.getVelocity();
+ for(; !steer_angle_it.isFinished(); steer_angle_it++)
+ {
+ sample[1] = steer_angle_it.getVelocity();
+ sample_params_.push_back(sample);
+ }
+ steer_angle_it.reset();
}
- steer_angle_it.reset();
}
}
-void AckermannTrajectoryGenerator::set_parameters(
- double max_sim_time,
- double min_sim_time,
- double sim_granularity,
- double angular_sim_granularity,
- bool use_steer_angle_cmd,
- double sim_period)
+void AckermannTrajectoryGenerator::reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg)
{
- max_sim_time_ = max_sim_time;
- min_sim_time_ = min_sim_time;
- sim_granularity_ = sim_granularity;
- angular_sim_granularity_ = angular_sim_granularity;
- sim_period_ = sim_period;
- use_steer_angle_cmd_ = use_steer_angle_cmd;
+ current_config_=cfg;
}
/**
@@ -308,7 +314,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
traj.cost_ = -1.0; // placed here in case we return early
//trajectory might be reused so we'll make sure to reset it
traj.resetPoints();
- int num_steps = int(sim_time_ / sim_granularity_ + 0.5);
+ int num_steps = int(sim_time_ / current_config_.sim_granularity + 0.5);
//we at least want to take one step... even if we won't move, we want to score our current position
if(num_steps == 0)
num_steps = 1;
@@ -317,22 +323,22 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
double T1=0.0,T4=0.0,T2=0.0,T3=0.0;
/* check wether the trajectory can be generated or not */
-/* if(steer_vel_i<0 && (-steer_vel_i*steer_vel_i/(2*limits_->max_steer_acc)+steer_angle_i)>limits_->max_steer_angle)
+/* if(steer_vel_i<0 && (-steer_vel_i*steer_vel_i/(2*current_config_.max_steer_acc)+steer_angle_i)>current_config_.max_steer_angle)
{
ROS_WARN("Impossible steering trajectory: speed: %f, angle: %f",steer_vel_i,steer_angle_i);
return false;
}
- if(steer_vel_i>0 && (steer_vel_i*steer_vel_i/(2*limits_->max_steer_acc)+steer_angle_i)>limits_->max_steer_angle)
+ if(steer_vel_i>0 && (steer_vel_i*steer_vel_i/(2*current_config_.max_steer_acc)+steer_angle_i)>current_config_.max_steer_angle)
{
ROS_WARN("Impossible steering trajectory: speed: %f, angle: %f",steer_vel_i,steer_angle_i);
return false;
}
- if(steer_vel_i<0 && (-steer_vel_i*steer_vel_i/(2*limits_->max_steer_acc)+steer_angle_i)<limits_->min_steer_angle)
+ if(steer_vel_i<0 && (-steer_vel_i*steer_vel_i/(2*current_config_.max_steer_acc)+steer_angle_i)<current_config_.min_steer_angle)
{
ROS_WARN("Impossible steering trajectory: speed: %f, angle: %f",steer_vel_i,steer_angle_i);
return false;
}
- if(steer_vel_i>0 && (steer_vel_i*steer_vel_i/(2*limits_->max_steer_acc)+steer_angle_i)<limits_->min_steer_angle)
+ if(steer_vel_i>0 && (steer_vel_i*steer_vel_i/(2*current_config_.max_steer_acc)+steer_angle_i)<current_config_.min_steer_angle)
{
ROS_WARN("Impossible steering trajectory: speed: %f, angle: %f",steer_vel_i,steer_angle_i);
return false;
@@ -342,145 +348,145 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
{
if(sample_target_vel[1]>steer_angle_i)
{
- speed=(steer_vel_i+limits_->max_steer_acc*sim_time_)/2.0;
- if(speed>=limits_->max_steer_vel)
+ speed=(steer_vel_i+current_config_.max_steer_acc*sim_time_)/2.0;
+ if(speed>=current_config_.max_steer_vel)
{
- speed=limits_->max_steer_vel;
- T4=sim_time_-(2.0*speed-steer_vel_i)/limits_->max_steer_acc;
- angle=speed*speed/limits_->max_steer_acc-steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc)+speed*sim_time_*T4+steer_angle_i;
+ speed=current_config_.max_steer_vel;
+ T4=sim_time_-(2.0*speed-steer_vel_i)/current_config_.max_steer_acc;
+ angle=speed*speed/current_config_.max_steer_acc-steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc)+speed*sim_time_*T4+steer_angle_i;
if(angle>sample_target_vel[1])
{
angle=sample_target_vel[1];
- T4=(sample_target_vel[1]-steer_angle_i-speed*speed/limits_->max_steer_acc+steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc))/speed;
+ T4=(sample_target_vel[1]-steer_angle_i-speed*speed/current_config_.max_steer_acc+steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc))/speed;
if(T4<0)
{
T4=0;
- speed=sqrt(steer_vel_i*steer_vel_i/2.0+limits_->max_steer_acc*(sample_target_vel[1]-steer_angle_i));
+ speed=sqrt(steer_vel_i*steer_vel_i/2.0+current_config_.max_steer_acc*(sample_target_vel[1]-steer_angle_i));
}
}
}
else
{
- angle=speed*speed/limits_->max_steer_acc-steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc)+steer_angle_i;
+ angle=speed*speed/current_config_.max_steer_acc-steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc)+steer_angle_i;
T4=0;
if(angle>sample_target_vel[1])
{
angle=sample_target_vel[1];
- speed=sqrt(steer_vel_i*steer_vel_i/2.0+limits_->max_steer_acc*(sample_target_vel[1]-steer_angle_i));
+ speed=sqrt(steer_vel_i*steer_vel_i/2.0+current_config_.max_steer_acc*(sample_target_vel[1]-steer_angle_i));
}
}
- T1=fabs(speed-steer_vel_i)/limits_->max_steer_acc;
+ T1=fabs(speed-steer_vel_i)/current_config_.max_steer_acc;
}
else
{
- speed=(steer_vel_i-limits_->max_steer_acc*sim_time_)/2.0;
- if(speed<=limits_->min_steer_vel)
+ speed=(steer_vel_i-current_config_.max_steer_acc*sim_time_)/2.0;
+ if(speed<=current_config_.min_steer_vel)
{
- speed=limits_->min_steer_vel;
- T4=sim_time_+(2.0*speed-steer_vel_i)/limits_->max_steer_acc;
- angle=steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc)-speed*speed/limits_->max_steer_acc+speed*T4+steer_angle_i;
+ speed=current_config_.min_steer_vel;
+ T4=sim_time_+(2.0*speed-steer_vel_i)/current_config_.max_steer_acc;
+ angle=steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc)-speed*speed/current_config_.max_steer_acc+speed*T4+steer_angle_i;
if(angle<sample_target_vel[1])
{
angle=sample_target_vel[1];
- T4=(sample_target_vel[1]-steer_angle_i+speed*speed/limits_->max_steer_acc-steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc))/speed;
+ T4=(sample_target_vel[1]-steer_angle_i+speed*speed/current_config_.max_steer_acc-steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc))/speed;
if(T4<0)
{
T4=0;
- speed=-sqrt(steer_vel_i*steer_vel_i/2.0-limits_->max_steer_acc*(sample_target_vel[1]-steer_angle_i));
+ speed=-sqrt(steer_vel_i*steer_vel_i/2.0-current_config_.max_steer_acc*(sample_target_vel[1]-steer_angle_i));
}
}
}
else
{
- angle=steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc)-speed*speed/limits_->max_steer_acc+steer_angle_i;
+ angle=steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc)-speed*speed/current_config_.max_steer_acc+steer_angle_i;
T4=0;
if(angle<sample_target_vel[1])
{
angle=sample_target_vel[1];
- speed=-sqrt(steer_vel_i*steer_vel_i/2.0-limits_->max_steer_acc*(sample_target_vel[1]-steer_angle_i));
+ speed=-sqrt(steer_vel_i*steer_vel_i/2.0-current_config_.max_steer_acc*(sample_target_vel[1]-steer_angle_i));
}
}
- T1=fabs(speed-steer_vel_i)/limits_->max_steer_acc;
+ T1=fabs(speed-steer_vel_i)/current_config_.max_steer_acc;
}
}
else
{
if(sample_target_vel[1]>steer_angle_i)
{
- speed=(steer_vel_i+limits_->max_steer_acc*sim_time_)/2.0;
- if(speed>=limits_->max_steer_vel)
+ speed=(steer_vel_i+current_config_.max_steer_acc*sim_time_)/2.0;
+ if(speed>=current_config_.max_steer_vel)
{
- speed=limits_->max_steer_vel;
- T4=sim_time_-(2.0*speed-steer_vel_i)/limits_->max_steer_acc;
- angle=-steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc)+speed*speed/limits_->max_steer_acc+speed*T4+steer_angle_i;
+ speed=current_config_.max_steer_vel;
+ T4=sim_time_-(2.0*speed-steer_vel_i)/current_config_.max_steer_acc;
+ angle=-steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc)+speed*speed/current_config_.max_steer_acc+speed*T4+steer_angle_i;
if(angle>sample_target_vel[1])
{
angle=sample_target_vel[1];
- T4=(sample_target_vel[1]-steer_angle_i-speed*speed/limits_->max_steer_acc+steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc))/speed;
+ T4=(sample_target_vel[1]-steer_angle_i-speed*speed/current_config_.max_steer_acc+steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc))/speed;
if(T4<0)
{
T4=0;
- speed=sqrt(steer_vel_i*steer_vel_i/2.0+limits_->max_steer_acc*(sample_target_vel[1]-steer_angle_i));
+ speed=sqrt(steer_vel_i*steer_vel_i/2.0+current_config_.max_steer_acc*(sample_target_vel[1]-steer_angle_i));
}
}
}
else
{
- angle=-steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc)+speed*speed/limits_->max_steer_acc+steer_angle_i;
+ angle=-steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc)+speed*speed/current_config_.max_steer_acc+steer_angle_i;
T4=0;
if(angle>sample_target_vel[1])
{
angle=sample_target_vel[1];
- speed=sqrt(steer_vel_i*steer_vel_i/2.0+limits_->max_steer_acc*(sample_target_vel[1]-steer_angle_i));
+ speed=sqrt(steer_vel_i*steer_vel_i/2.0+current_config_.max_steer_acc*(sample_target_vel[1]-steer_angle_i));
}
}
- T1=fabs(speed-steer_vel_i)/limits_->max_steer_acc;
+ T1=fabs(speed-steer_vel_i)/current_config_.max_steer_acc;
}
else
{
- speed=(steer_vel_i-limits_->max_steer_acc*sim_time_)/2.0;
- if(speed<=limits_->min_steer_vel)
+ speed=(steer_vel_i-current_config_.max_steer_acc*sim_time_)/2.0;
+ if(speed<=current_config_.min_steer_vel)
{
- speed=limits_->min_steer_vel;
- T4=sim_time_+(2.0*speed-steer_vel_i)/limits_->max_steer_acc;
- angle=-speed*speed/limits_->max_steer_acc+steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc)+speed*T4+steer_angle_i;
+ speed=current_config_.min_steer_vel;
+ T4=sim_time_+(2.0*speed-steer_vel_i)/current_config_.max_steer_acc;
+ angle=-speed*speed/current_config_.max_steer_acc+steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc)+speed*T4+steer_angle_i;
if(angle<sample_target_vel[1])
{
angle=sample_target_vel[1];
- T4=(sample_target_vel[1]-steer_angle_i+speed*speed/limits_->max_steer_acc-steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc))/speed;
+ T4=(sample_target_vel[1]-steer_angle_i+speed*speed/current_config_.max_steer_acc-steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc))/speed;
if(T4<0)
{
T4=0;
- speed=-sqrt(steer_vel_i*steer_vel_i/2.0-limits_->max_steer_acc*(sample_target_vel[1]-steer_angle_i));
+ speed=-sqrt(steer_vel_i*steer_vel_i/2.0-current_config_.max_steer_acc*(sample_target_vel[1]-steer_angle_i));
}
}
}
else
{
- angle=-speed*speed/limits_->max_steer_acc+steer_vel_i*steer_vel_i/(2.0*limits_->max_steer_acc)+steer_angle_i;
+ angle=-speed*speed/current_config_.max_steer_acc+steer_vel_i*steer_vel_i/(2.0*current_config_.max_steer_acc)+steer_angle_i;
T4=0;
if(angle<sample_target_vel[1])
{
angle=sample_target_vel[1];
- speed=-sqrt(steer_vel_i*steer_vel_i/2.0-limits_->max_steer_acc*(sample_target_vel[1]-steer_angle_i));
+ speed=-sqrt(steer_vel_i*steer_vel_i/2.0-current_config_.max_steer_acc*(sample_target_vel[1]-steer_angle_i));
}
}
- T1=fabs(speed-steer_vel_i)/limits_->max_steer_acc;
+ T1=fabs(speed-steer_vel_i)/current_config_.max_steer_acc;
}
}
double v=0.0;
if(sample_target_vel[0]>trans_vel_i)
{
- v=(trans_vel_i+limits_->max_trans_acc*sim_time_)/2.0;
+ v=(trans_vel_i+current_config_.max_trans_acc*sim_time_)/2.0;
if(v>sample_target_vel[0])
{
v=sample_target_vel[0];
- T3=sim_time_-(2.0*v-trans_vel_i)/limits_->max_trans_acc;
+ T3=sim_time_-(2.0*v-trans_vel_i)/current_config_.max_trans_acc;
}
else
T3=0;
- T2=(v-trans_vel_i)/limits_->max_trans_acc;
+ T2=(v-trans_vel_i)/current_config_.max_trans_acc;
if(T2<0)
{
ROS_WARN("Impossible drive trajectory: v_trans: %f",trans_vel_i);
@@ -489,15 +495,15 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
}
else
{
- v=(trans_vel_i-limits_->max_trans_acc*sim_time_)/2.0;
+ v=(trans_vel_i-current_config_.max_trans_acc*sim_time_)/2.0;
if(v<sample_target_vel[0])
{
v=sample_target_vel[0];
- T3=sim_time_+(2.0*v-trans_vel_i)/limits_->max_trans_acc;
+ T3=sim_time_+(2.0*v-trans_vel_i)/current_config_.max_trans_acc;
}
else
T3=0;
- T2=-(v-trans_vel_i)/limits_->max_trans_acc;
+ T2=-(v-trans_vel_i)/current_config_.max_trans_acc;
if(T2<0)
{
ROS_WARN("Impossible drive trajectory: v_trans: %f",trans_vel_i);
@@ -523,7 +529,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
{
if(time<T1)
{
- steer_vel_i=steer_vel_i+limits_->max_steer_acc*dt;
+ steer_vel_i=steer_vel_i+current_config_.max_steer_acc*dt;
if(steer_vel_i>speed)
steer_vel_i=speed;
}
@@ -531,7 +537,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
steer_vel_i=speed;
else
{
- steer_vel_i=steer_vel_i-limits_->max_steer_acc*dt;
+ steer_vel_i=steer_vel_i-current_config_.max_steer_acc*dt;
if(steer_vel_i<0)
steer_vel_i=0;
}
@@ -540,7 +546,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
{
if(time<T1)
{
- steer_vel_i=steer_vel_i-limits_->max_steer_acc*dt;
+ steer_vel_i=steer_vel_i-current_config_.max_steer_acc*dt;
if(steer_vel_i<speed)
steer_vel_i=speed;
}
@@ -548,7 +554,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
steer_vel_i=speed;
else
{
- steer_vel_i=steer_vel_i+limits_->max_steer_acc*dt;
+ steer_vel_i=steer_vel_i+current_config_.max_steer_acc*dt;
if(steer_vel_i>0)
steer_vel_i=0;
}
@@ -558,7 +564,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
{
if(time<T2)
{
- trans_vel_i=trans_vel_i+limits_->max_trans_acc*dt;
+ trans_vel_i=trans_vel_i+current_config_.max_trans_acc*dt;
if(trans_vel_i>v)
trans_vel_i=v;
}
@@ -566,7 +572,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
trans_vel_i=v;
else
{
- trans_vel_i=trans_vel_i-limits_->max_trans_acc*dt;
+ trans_vel_i=trans_vel_i-current_config_.max_trans_acc*dt;
if(trans_vel_i<0)
trans_vel_i=0;
}
@@ -575,7 +581,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
{
if(time<T2)
{
- trans_vel_i=trans_vel_i-limits_->max_trans_acc*dt;
+ trans_vel_i=trans_vel_i-current_config_.max_trans_acc*dt;
if(trans_vel_i<v)
trans_vel_i=v;
}
@@ -583,7 +589,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
trans_vel_i=v;
else
{
- trans_vel_i=trans_vel_i+limits_->max_trans_acc*dt;
+ trans_vel_i=trans_vel_i+current_config_.max_trans_acc*dt;
if(trans_vel_i>0)
trans_vel_i=0;
}
@@ -591,7 +597,7 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
double r,d;
if(fabs(steer_angle_i)>0.001)
{
- r=fabs(limits_->axis_distance/tan(steer_angle_i));
+ r=fabs(current_config_.axis_distance/tan(steer_angle_i));
d=trans_vel_i*dt;
if(steer_angle_i>0)
theta_i+=d/r;
@@ -609,20 +615,24 @@ bool AckermannTrajectoryGenerator::generate_trajectory(
time+=dt;
} // end for simulation steps
- if(this->use_steer_angle_cmd_)
+ if(this->current_config_.use_steer_angle_cmd)
{
traj.thetav_ = sample_target_vel[1];
- if(traj.thetav_>limits_->max_angular_vel)
- traj.thetav_=limits_->max_angular_vel;
- else if(traj.thetav_<limits_->min_angular_vel)
- traj.thetav_=limits_->min_angular_vel;
+ if(traj.thetav_>current_config_.max_steer_angle)
+ traj.thetav_=current_config_.max_steer_angle;
+ else if(traj.thetav_<current_config_.min_steer_angle)
+ traj.thetav_=current_config_.min_steer_angle;
}
else
{
if(fabs(sample_target_vel[1])>0.0)
{
- double radius=limits_->axis_distance/tan(sample_target_vel[1]);
+ double radius=current_config_.axis_distance/tan(sample_target_vel[1]);
traj.thetav_ = sample_target_vel[0]/radius;
+ if(traj.thetav_>current_config_.max_angular_vel)
+ traj.thetav_=current_config_.max_angular_vel;
+ else if(traj.thetav_<current_config_.min_angular_vel)
+ traj.thetav_=current_config_.min_angular_vel;
}
else
traj.thetav_ = 0.0;
diff --git a/src/ackermann_trajectory_search.cpp b/src/ackermann_trajectory_search.cpp
index ceb10bfcc010babf594736516ebe0d08d0a6bed0..7ba497220a48054eab38d3c50e072c3485a6e02e 100644
--- a/src/ackermann_trajectory_search.cpp
+++ b/src/ackermann_trajectory_search.cpp
@@ -36,9 +36,11 @@
*********************************************************************/
#include <ackermann_trajectory_search.h>
-
#include <ros/console.h>
+#include "g2_spline.h"
+#include "vel_trapezoid.h"
+
AckermannTrajectorySearch::AckermannTrajectorySearch(std::vector<base_local_planner::TrajectorySampleGenerator*> gen_list, std::vector<base_local_planner::TrajectoryCostFunction*>& critics, int max_samples)
{
max_samples_ = max_samples;
@@ -50,6 +52,7 @@ double AckermannTrajectorySearch::scoreTrajectory(base_local_planner::Trajectory
{
double traj_cost = 0;
int gen_id = 0;
+
for(std::vector<base_local_planner::TrajectoryCostFunction*>::iterator score_function = critics_.begin(); score_function != critics_.end(); ++score_function)
{
base_local_planner::TrajectoryCostFunction* score_function_p = *score_function;
@@ -69,7 +72,7 @@ double AckermannTrajectorySearch::scoreTrajectory(base_local_planner::Trajectory
{
// since we keep adding positives, once we are worse than the best, we will stay worse
if (traj_cost > best_traj_cost)
- break;
+ break;
}
gen_id ++;
ROS_DEBUG("Velocity %.3lf, %.3lf, %.3lf evaluated by cost function %d with cost: %f (best cost: %f)", traj.xv_, traj.yv_, traj.thetav_, gen_id, cost,best_traj_cost);
@@ -85,6 +88,7 @@ bool AckermannTrajectorySearch::findBestTrajectory(base_local_planner::Trajector
double loop_traj_cost, best_traj_cost = -1;
bool gen_success;
int count, count_valid,num=0;
+
for (std::vector<base_local_planner::TrajectoryCostFunction*>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end(); ++loop_critic)
{
base_local_planner::TrajectoryCostFunction* loop_critic_p = *loop_critic;
@@ -125,10 +129,7 @@ bool AckermannTrajectorySearch::findBestTrajectory(base_local_planner::Trajector
}
count++;
if (max_samples_ > 0 && count >= max_samples_)
- {
- std::cout << "Max sample limit" << std::endl;
break;
- }
}
if (best_traj_cost >= 0)
{
diff --git a/src/heading_cost_function.cpp b/src/heading_cost_function.cpp
index d7b1846cb51f9670db0e85d2ecdc7092dbb25b01..0770792a56a230848a9ce0f151f074fdb147c0f8 100644
--- a/src/heading_cost_function.cpp
+++ b/src/heading_cost_function.cpp
@@ -95,7 +95,7 @@ double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj
diff=fabs(diff-3.14159);
heading_diff+=diff;
}
- heading_diff/=this->num_points;
+ heading_diff/=samples;
return heading_diff;
}