Merge branch...

Merge branch '1-list-of-bugs-discovered-developing-follow_local_planner-branch-splines' into 'splines'

Fixed listed bugs

See merge request !6

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)

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);
 };

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

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
 

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_)
   {

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;

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");

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

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;
 }