diff --git a/cfg/AckermannLocalPlanner.cfg b/cfg/AckermannLocalPlanner.cfg
index 17772ce84b0bcfd98a64870f884ac682c79e2503..52e2a1c6d36e34a2752b9879bd11d5d427c6200e 100755
--- a/cfg/AckermannLocalPlanner.cfg
+++ b/cfg/AckermannLocalPlanner.cfg
@@ -62,6 +62,7 @@ planner.add("trans_vel_deadzone", double_t, 0, "Translatinal velocity mini
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)
diff --git a/include/ackermann_spline_generator.h b/include/ackermann_spline_generator.h
index f9992d37015edf3c7a84e7c39f64ad2b7dd99fbe..05b65687c80d0c2927a9c4a52b253b5ad4abc864 100644
--- a/include/ackermann_spline_generator.h
+++ b/include/ackermann_spline_generator.h
@@ -92,6 +92,7 @@ class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGener
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);
};
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_planner.cpp b/src/ackermann_planner.cpp
index 0059999304cf22fc1f5b747eb36564ec4009f468..1847eecd2998f4685804904242f57c65ddf89ff8 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -56,14 +56,14 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
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);
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;
@@ -77,7 +77,10 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
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
{
double controller_frequency = 0;
@@ -86,7 +89,7 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
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");
+ 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;
}
}
@@ -136,7 +139,10 @@ AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planne
std::string controller_frequency_param_name;
double sim_period;
if(!this->private_nh.searchParam("controller_frequency", controller_frequency_param_name))
+ {
+ 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;
@@ -257,8 +263,8 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
for(double angle=0;angle<2*3.14159;angle+=0.1)
{
geometry_msgs::Point new_point;
- new_point.x=(max_radius+0.25)*cos(angle);
- new_point.y=(max_radius+0.25)*sin(angle);
+ 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{
@@ -287,14 +293,18 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
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));
+ 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);
- spline_generator_.initialise(pos,ack_state,goal,plan_forward,global_plan_);
+ 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_)
{
diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index 75bde7fe6501d9d9128ae4ddf4a890f2dbf473a1..76c23a8b2d472409c162e7ad9efbbefa5c5cb009 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/src/ackermann_planner_ros.cpp
@@ -104,9 +104,14 @@ void AckermannPlannerROS::initialize(std::string name,tf2_ros::Buffer* tf,costma
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;
diff --git a/src/ackermann_planner_util.cpp b/src/ackermann_planner_util.cpp
index e916a49575c008ab624be8bdc3f8ff1b311bfacb..34dc68931e1f3c6c039793a49483fdfc237c8c39 100644
--- a/src/ackermann_planner_util.cpp
+++ b/src/ackermann_planner_util.cpp
@@ -234,8 +234,11 @@ bool AckermannPlannerUtil::last_path(void)
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");
diff --git a/src/ackermann_spline_generator.cpp b/src/ackermann_spline_generator.cpp
index 6a88e0218dc60d03df70e48d1b7843aa9759135e..8a88edb528b9a251567a8317bbf5d933160dde50 100644
--- a/src/ackermann_spline_generator.cpp
+++ b/src/ackermann_spline_generator.cpp
@@ -81,6 +81,7 @@ void AckermannSplineGenerator::initialise(
{
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;
@@ -120,22 +121,48 @@ void AckermannSplineGenerator::initialise(
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;
- max_n2=2.0*diff_y;
- for(double current_n1=min_n1;current_n1<=max_n1;current_n1+=(max_n1-min_n1)/(current_config_.n1_samples-1))
- for(double current_n2=min_n2;current_n2<=max_n2;current_n2+=(max_n2-min_n2)/(current_config_.n2_samples-1))
- for(double current_n3=min_n3;current_n3<=max_n3;current_n3+=(max_n3-min_n3)/(current_config_.n3_samples-1))
- for(double current_n4=min_n4;current_n4<=max_n4;current_n4+=(max_n4-min_n4)/(current_config_.n4_samples-1))
+ 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=current_plan[int_step-1].pose.position.x;
- new_sample.end_point.y=current_plan[int_step-1].pose.position.y;
- new_sample.end_point.heading=tf::getYaw(current_plan[int_step-1].pose.orientation);
+ 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)
@@ -196,7 +223,7 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
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,path_x,path_y;
+ double acceleration,length,steer_angle;
CVelTrapezoid vel_profile;
TPoint point;
@@ -210,7 +237,7 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
try{
vel_profile.evaluate_at_time(current_config_.temp_horizon,comp_traj.xv_,acceleration,length);
}catch(CException &e){
- length=0.0;
+ length=spline_.get_length();
comp_traj.xv_=vel_profile.get_max_vel();
}
if(!this->forward)
@@ -222,8 +249,6 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
comp_traj.thetav_-=2*3.14159;
else if(comp_traj.thetav_<-3.14159)
comp_traj.thetav_+=2*3.14159;
- this->get_path_closest_pose(path_x,path_y);
- double y_offset=(-(path_x-pos_(0))*sin(pos_(2))+(path_y-pos_(1))*cos(pos_(2)));
if(forward)
comp_traj.thetav_-=atan2(current_config_.lateral_offset_gain*y_offset,comp_traj.xv_);
else
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;
}