diff --git a/cfg/AckermannLocalPlanner.cfg b/cfg/AckermannLocalPlanner.cfg
index 97309a7c621f8b19014c84086f56dc87c04c7bbc..2e8f1bdafeffaedfdbb587b274b7f1582d9c22fe 100755
--- a/cfg/AckermannLocalPlanner.cfg
+++ b/cfg/AckermannLocalPlanner.cfg
@@ -44,18 +44,14 @@ gen = ParameterGenerator()
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)
diff --git a/include/ackermann_planner.h b/include/ackermann_planner.h
index ba93e7247fd3de29f4e28551f19b0cf25a8b0b41..360bfe8afebb171325bd029c82d12a9115c7a8b2 100644
--- a/include/ackermann_planner.h
+++ b/include/ackermann_planner.h
@@ -50,7 +50,6 @@
#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_odom_helper.h>
@@ -112,12 +111,6 @@ class AckermannPlanner
*/
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);
-
/**
* @brief Compute the components and total cost for a map grid cell
* @param cx The x coordinate of the cell in the map grid
@@ -138,16 +131,13 @@ 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_;
+ double pdist_scale_, gdist_scale_, occdist_scale_;
Eigen::Vector2f vsamples_;
- 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_;
boost::mutex configuration_mutex_;
@@ -156,8 +146,6 @@ class AckermannPlanner
bool publish_cost_grid_pc_; ///< @brief Whether or not to build and publish a PointCloud
bool publish_traj_pc_;
- double cheat_factor_;
-
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
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..862cc76015883d886839c803fae87e281f6584e3 100644
--- a/include/ackermann_planner_util.h
+++ b/include/ackermann_planner_util.h
@@ -46,9 +46,7 @@
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/PoseStamped.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.
@@ -65,23 +63,22 @@ class AckermannPlannerUtil
std::vector<geometry_msgs::PoseStamped> global_plan_;
- 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;
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);
@@ -97,8 +94,6 @@ class AckermannPlannerUtil
costmap_2d::Costmap2D* get_costmap();
- AckermannPlannerLimits get_current_limits();
-
std::string get_global_frame(void);
~AckermannPlannerUtil();
diff --git a/include/ackermann_trajectory_generator.h b/include/ackermann_trajectory_generator.h
index 26e6a3f6a883c7f9a8ec7ae1e54922c857abf60e..3047045e63c38dc4b84ef7c34ee06a65281b2653 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,17 @@ 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,
+ iri_ackermann_local_planner::AckermannLocalPlannerConfig &config,
+ const Eigen::Vector2f& vsamples,
+ bool discretize_by_time = false);
/**
- * 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 +96,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 +107,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/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index 371634056d22b0706be91c41ba8e31dc9b5c266a..87b5a167b4ffc27089bdb7cfeedbe25ed62bb772 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -51,22 +51,18 @@
void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &config)
{
boost::mutex::scoped_lock l(configuration_mutex_);
+ double hdiff_scale;
- 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_);
+ generator_.reconfigure(config);
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_.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);
@@ -75,10 +71,7 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
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);
@@ -109,6 +102,8 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
vsamples_[0] = trans_vel_samp;
vsamples_[1] = steer_angle_angular_vel_samp;
+
+ current_config_=config;
}
AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planner_util) :
@@ -119,29 +114,6 @@ AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planne
{
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))
- {
- 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
- {
- ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
- sim_period_ = 0.05;
- }
- }
-
oscillation_costs_.resetOscillationFlags();
bool sum_scores;
@@ -170,8 +142,6 @@ AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planne
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
@@ -209,11 +179,10 @@ bool AckermannPlanner::check_trajectory(Eigen::Vector3f pos,Eigen::Vector3f acke
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,
+ current_config_,
vsamples_);
generator_.generate_trajectory(pos, ackermann, samples, traj);
double cost = scored_sampling_planner_.scoreTrajectory(traj, -1);
@@ -259,13 +228,12 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
Eigen::Vector3f ack_state(ackermann.speed,ackermann.steering_angle,ackermann.steering_angle_velocity);
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();
// prepare cost functions and generators for this run
generator_.initialise(pos,
ack_state,
goal,
- &limits,
+ current_config_,
vsamples_);
result_traj_.cost_ = -7;
@@ -324,7 +292,7 @@ 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)
diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index c770851c21a041f78058b9c8f749d4cf4e1dcf70..ca394c7320c8b307005fcfd2203be3c783802fb7 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,10 @@ 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_ );
- }
+
if( private_nh.getParam( "ackermann_topic", ackermann_topic_ ))
- {
odom_helper_.set_ackermann_topic( ackermann_topic_ );
- }
initialized_ = true;
@@ -153,9 +117,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 +157,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 +184,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 +198,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 +238,6 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
bool AckermannPlannerROS::isGoalReached(void)
{
nav_msgs::Odometry odom;
- AckermannPlannerLimits limits;
if (! is_initialized()) {
ROS_ERROR("AckermannPlannerROS: this planner has not been initialized, please call initialize() before using this planner");
@@ -295,7 +254,6 @@ bool AckermannPlannerROS::isGoalReached(void)
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 +262,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;
@@ -350,7 +308,6 @@ 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;
@@ -402,15 +359,14 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
dp_->update_plan_and_local_costs(current_pose_, transformed_plan);
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))
+ current_config_.rot_stopped_vel,current_config_.trans_stopped_vel,
+ current_config_.xy_goal_tolerance,current_config_.yaw_goal_tolerance))
{
if(planner_util_.set_next_path())
{
@@ -453,17 +409,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: 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,9 +438,9 @@ 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,
@@ -492,10 +448,10 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
goal_pose);
// 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 +460,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;
@@ -528,12 +484,13 @@ 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)
+ // TODO: apply the deadzone before the trajectory generation and evaluation
+ if(current_config_.use_trans_vel_deadzone && fabs(cmd_vel.linear.x)<current_config_.trans_vel_deadzone)
{
if(cmd_vel.linear.x>0.0)
- cmd_vel.linear.x=limits.trans_vel_deadzone;
+ cmd_vel.linear.x=current_config_.trans_vel_deadzone;
else if(cmd_vel.linear.x<0.0)
- cmd_vel.linear.x=-limits.trans_vel_deadzone;
+ cmd_vel.linear.x=-current_config_.trans_vel_deadzone;
}
}
else
diff --git a/src/ackermann_planner_util.cpp b/src/ackermann_planner_util.cpp
index 11254acddd7b05a3076e7eef7445407690fc01cc..9b97565c0f5487d42a516bd98e7c961a668ff2b2 100644
--- a/src/ackermann_planner_util.cpp
+++ b/src/ackermann_planner_util.cpp
@@ -59,18 +59,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,12 +79,6 @@ costmap_2d::Costmap2D* AckermannPlannerUtil::get_costmap()
return costmap_;
}
-AckermannPlannerLimits AckermannPlannerUtil::get_current_limits()
-{
- boost::mutex::scoped_lock l(limits_configuration_mutex_);
- return limits_;
-}
-
std::string AckermannPlannerUtil::get_global_frame(void)
{
return global_frame_;
@@ -137,7 +132,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
angle=std::acos(angle);
if(fabs(angle)>1.57)
{
- if(pathLength>limits_.split_ignore_length)
+ if(pathLength>current_config_.split_ignore_length)
{
//ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, length=%f", i, angle, pathLength);
subPathList.push_back(subPath);
@@ -199,7 +194,7 @@ 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_);
return true;
diff --git a/src/ackermann_trajectory_generator.cpp b/src/ackermann_trajectory_generator.cpp
index 789c1bec6b67391fa0c1da31ac61dc6e83f0d332..0050ba63c0a191a85f83845792c8d80382c07be5 100644
--- a/src/ackermann_trajectory_generator.cpp
+++ b/src/ackermann_trajectory_generator.cpp
@@ -47,14 +47,13 @@
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,
+ iri_ackermann_local_planner::AckermannLocalPlannerConfig &config,
const Eigen::Vector2f& vsamples,//trans_vel samples & steer_angle samples
bool discretize_by_time)
{
@@ -67,166 +66,166 @@ void AckermannTrajectoryGenerator::initialise(
* We actually generate all velocity sample vectors here, from which to generate trajectories later on
*/
pos_ = pos;
- limits_ = limits;
+ current_config_ = config;
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]);
@@ -243,20 +242,9 @@ void AckermannTrajectoryGenerator::initialise(
}
}
-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 +296,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 +305,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 +330,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 +477,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 +511,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 +519,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 +528,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 +536,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 +546,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 +554,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 +563,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 +571,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 +579,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,19 +597,19 @@ 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_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
{
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;
}
else