From 6331d093b2e75aaec303cd3a3e576efe5b791d13 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 19 Oct 2022 12:49:16 +0200
Subject: [PATCH] When splitting the input path, the forward or backward
direction of each segment is caomputed. The velocity dead zone is applied
when generating the velocity samples to check. The past cost uses all the
path points, not only the last one. Version using splines operational in
simulation.
---
include/ackermann_planner.h | 3 +-
include/ackermann_planner_util.h | 4 ++-
include/ackermann_spline_generator.h | 3 +-
src/ackermann_planner.cpp | 30 ++++++++----------
src/ackermann_planner_ros.cpp | 18 +++--------
src/ackermann_planner_util.cpp | 38 ++++++++++++++++++++--
src/ackermann_spline_generator.cpp | 44 ++++++++++++++++----------
src/ackermann_trajectory_generator.cpp | 13 +++++---
src/ackermann_trajectory_search.cpp | 9 ++----
9 files changed, 97 insertions(+), 65 deletions(-)
diff --git a/include/ackermann_planner.h b/include/ackermann_planner.h
index f07e6b9..496f784 100644
--- a/include/ackermann_planner.h
+++ b/include/ackermann_planner.h
@@ -101,7 +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);
+ 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
@@ -130,6 +130,7 @@ class AckermannPlanner
base_local_planner::Trajectory result_traj_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
+ bool plan_forward;
boost::mutex configuration_mutex_;
std::string frame_id_;
diff --git a/include/ackermann_planner_util.h b/include/ackermann_planner_util.h
index 862cc76..1025232 100644
--- a/include/ackermann_planner_util.h
+++ b/include/ackermann_planner_util.h
@@ -62,11 +62,13 @@ class AckermannPlannerUtil
tf2_ros::Buffer *tf_;
std::vector<geometry_msgs::PoseStamped> global_plan_;
+ bool plan_forward;
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_;
@@ -90,7 +92,7 @@ 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();
diff --git a/include/ackermann_spline_generator.h b/include/ackermann_spline_generator.h
index e30b934..46d1c3e 100644
--- a/include/ackermann_spline_generator.h
+++ b/include/ackermann_spline_generator.h
@@ -69,7 +69,7 @@ class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGener
*/
bool nextTrajectory(base_local_planner::Trajectory &traj);
- void initialise(const Eigen::Vector3f& pos,const Eigen::Vector3f& ackermann,const Eigen::Vector3f& goal);
+ void initialise(const Eigen::Vector3f& pos,const Eigen::Vector3f& ackermann,const Eigen::Vector3f& goal,bool forward);
void set_sim_period(double period);
@@ -82,6 +82,7 @@ class AckermannSplineGenerator: public base_local_planner::TrajectorySampleGener
Eigen::Vector3f pos_;
Eigen::Vector3f goal_pos_;
Eigen::Vector3f ackermann_;
+ bool forward;
double sim_period_;
};
diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp
index 8bcada8..dc83b21 100644
--- a/src/ackermann_planner.cpp
+++ b/src/ackermann_planner.cpp
@@ -83,7 +83,7 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl
AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planner_util) :
planner_util_(planner_util),
obstacle_costs_(planner_util->get_costmap()),
- path_costs_(planner_util->get_costmap()),
+ 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)
{
ros::NodeHandle private_nh("~/" + name);
@@ -176,11 +176,12 @@ bool AckermannPlanner::set_plan(const std::vector<geometry_msgs::PoseStamped>& o
return planner_util_->set_plan(orig_global_plan);
}
-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_);
@@ -189,10 +190,6 @@ void AckermannPlanner::update_plan_and_local_costs(geometry_msgs::PoseStamped &g
goal_costs_.setTargetPoses(global_plan_);
heading_costs_.set_global_plan(global_plan_);
-
- // alignment costs
- geometry_msgs::PoseStamped goal_pose = global_plan_.back();
-
}
/*
@@ -212,14 +209,13 @@ base_local_planner::Trajectory AckermannPlanner::find_best_path(geometry_msgs::P
// prepare cost functions and generators for this run
generator_.initialise(pos,ack_state,goal);
- spline_generator_.initialise(pos,ack_state,goal);
+ spline_generator_.initialise(pos,ack_state,goal,plan_forward);
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);
- std::cout << "all explored: " << all_explored.size() << std::endl;
if(publish_traj_pc_)
{
sensor_msgs::PointCloud2 traj_cloud;
@@ -287,18 +283,18 @@ 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.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.x = result_traj_.xv_;
+ drive_velocities.pose.position.y = result_traj_.yv_;
drive_velocities.pose.position.z = 0.0;
tf2::Quaternion q;
- q.setRPY(0, 0, 0);
+ 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 6f8c488..8ebd7d5 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/src/ackermann_planner_ros.cpp
@@ -239,6 +239,7 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
bool AckermannPlannerROS::isGoalReached(void)
{
nav_msgs::Odometry odom;
+ bool forward;
if (! is_initialized()) {
ROS_ERROR("AckermannPlannerROS: this planner has not been initialized, please call initialize() before using this planner");
@@ -249,7 +250,7 @@ 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;
@@ -313,6 +314,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
static int stuck_count=0;
static int replan_count=0;
static bool new_segment=false;
+ bool forward;
// 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_))
@@ -328,7 +330,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;
@@ -357,7 +359,7 @@ 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);
odom_helper_.get_odom(odom);
if(this->stuck || base_local_planner::isGoalReached(*tf_,
@@ -483,17 +485,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
}
if (isOk)
- {
publish_global_plan(transformed_plan);
- // 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=current_config_.trans_vel_deadzone;
- else if(cmd_vel.linear.x<0.0)
- cmd_vel.linear.x=-current_config_.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 a9f1909..d79cd19 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>
@@ -99,8 +100,9 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
////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)
{
if(i>2 && i<(orig_global_plan.size()-1))
@@ -118,7 +120,7 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
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;
+// std::cout << curvature << std::endl;
if(angle<-1.0)
angle=-1.0;
else if(angle>1.0)
@@ -138,6 +140,19 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
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
@@ -151,6 +166,19 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
}
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);
ROS_INFO("AckermannPlannerUtil::setPlan: Global plan (%lu points) split in %lu paths", orig_global_plan.size(), subPathList.size());
@@ -159,6 +187,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;
}
@@ -171,6 +200,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
@@ -186,7 +216,7 @@ 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);
@@ -200,6 +230,8 @@ bool AckermannPlannerUtil::get_local_plan(geometry_msgs::PoseStamped& global_pos
if(current_config_.prune_plan)
base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
+ forward=plan_forward;
+
return true;
}
diff --git a/src/ackermann_spline_generator.cpp b/src/ackermann_spline_generator.cpp
index 0867043..c29da3b 100644
--- a/src/ackermann_spline_generator.cpp
+++ b/src/ackermann_spline_generator.cpp
@@ -51,15 +51,15 @@ AckermannSplineGenerator::AckermannSplineGenerator()
}
void AckermannSplineGenerator::initialise(
- const Eigen::Vector3f& pos,
- const Eigen::Vector3f& ackermann,//[0] -> trans_vel, [1]-> steer_angle, [2]-> steer_vel
- const Eigen::Vector3f& goal)
+ const Eigen::Vector3f& pos,
+ const Eigen::Vector3f& ackermann,//[0] -> trans_vel, [1]-> steer_angle, [2]-> steer_vel
+ const Eigen::Vector3f& goal,
+ bool forward)
{
double diff_x,diff_y;
double min_n1,max_n1,min_n2,max_n2,min_n3,max_n3,min_n4,max_n4;
double current_n1,current_n2,current_n3,current_n4;
- std::cout << "initialize" << std::endl;
pos_=pos;
ackermann_=ackermann;
goal_pos_=goal;
@@ -79,11 +79,8 @@ void AckermannSplineGenerator::initialise(
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))
- {
-
samples_.push_back(Eigen::Vector4f(current_n1,current_n2,current_n3,current_n4));
- std::cout << "generate: " << current_n1 << "," << current_n2 << "," << current_n3 << "," << current_n4 << std::endl;
- }
+ this->forward=forward;
}
void AckermannSplineGenerator::reconfigure(iri_ackermann_local_planner::AckermannLocalPlannerConfig &cfg)
@@ -110,19 +107,25 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
start.x=pos_(0);
start.y=pos_(1);
- start.heading=pos_(2);
+ 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;;
+// std::cout << "start position: " << pos_(0) << "," << pos_(1) << "," << pos_(2) << "," << start.curvature << std::endl;
end.x=goal_pos_(0);
end.y=goal_pos_(1);
- end.heading=goal_pos_(2);
+ if(this->forward)
+ end.heading=goal_pos_(2);
+ else
+ end.heading=goal_pos_(2)+3.14159;
end.curvature=0.0;
spline_.set_end_point(end);
- std::cout << "end position: " << goal_pos_(0) << "," << goal_pos_(1) << "," << goal_pos_(2) << "," << end.curvature << std::endl;
+// std::cout << "end position: " << goal_pos_(0) << "," << goal_pos_(1) << "," << goal_pos_(2) << "," << end.curvature << std::endl;
resolution=current_config_.sim_granularity;
spline_.generate(resolution,samples_[next_sample_index_](0),samples_[next_sample_index_](1),samples_[next_sample_index_](2),samples_[next_sample_index_](3));
spline_.evaluate_all(x,y,curvature,heading);
@@ -132,7 +135,7 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
for(unsigned int i=0;i<curvature.size();i++)
if(curvature[i]>k_max)
k_max=curvature[i];
- max_curvature=tan(std::max(current_config_.max_steer_angle,fabs(current_config_.min_steer_angle)))/current_config_.axis_distance;
+ max_curvature=tan(std::max(current_config_.max_steer_angle,fabs(current_config_.min_steer_angle)))/fabs(current_config_.axis_distance);
next_sample_index_++;
// if(k_max>max_curvature)
// {
@@ -152,11 +155,13 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
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);
- std::cout << ackermann_(0) << "," << min_max_speed << "," << current_config_.max_trans_vel << "," << k_max_speed << std::endl;
+// std::cout << ackermann_(0) << "," << min_max_speed << "," << current_config_.max_trans_vel << "," << k_max_speed << std::endl;
try{
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))
{
vel_profile.evaluate_at_time(sim_period_,comp_traj.xv_,acceleration,length);
+ if(!this->forward)
+ comp_traj.xv_*=-1.0;
std::cout << "speed at " << sim_period_ << ": " << comp_traj.xv_ << std::endl;
comp_traj.yv_=0.0;
point=spline_.evaluate(length);
@@ -166,9 +171,14 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
else
steer_angle=atan2(current_config_.axis_distance,1.0/point.curvature);
std::cout << "steer angle: " << steer_angle << std::endl;
+ if(steer_angle>1.5707)
+ steer_angle-=3.14159;
if(current_config_.use_steer_angle_cmd)
{
- comp_traj.thetav_ = steer_angle;
+ if(forward)
+ comp_traj.thetav_ = steer_angle;
+ else
+ comp_traj.thetav_ = -steer_angle;
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)
@@ -180,14 +190,14 @@ bool AckermannSplineGenerator::nextTrajectory(base_local_planner::Trajectory &co
for(unsigned int i=0;i<x.size();i++)
{
comp_traj.addPoint(x[i],y[i],heading[i]);
- std::cout << x[i] << "," << y[i] << "," << heading[i] << "," << curvature[i] << std::endl;
+// std::cout << x[i] << "," << y[i] << "," << heading[i] << "," << curvature[i] << std::endl;
}
return true;
}
else
return false;
}catch(CException &e){
- ROS_INFO_STREAM(e.what());
+ //ROS_INFO_STREAM(e.what());
return false;
}
// }
diff --git a/src/ackermann_trajectory_generator.cpp b/src/ackermann_trajectory_generator.cpp
index c295941..85a77ca 100644
--- a/src/ackermann_trajectory_generator.cpp
+++ b/src/ackermann_trajectory_generator.cpp
@@ -247,13 +247,16 @@ void AckermannTrajectoryGenerator::initialise(
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();
}
}
diff --git a/src/ackermann_trajectory_search.cpp b/src/ackermann_trajectory_search.cpp
index 0da3643..7ba4972 100644
--- a/src/ackermann_trajectory_search.cpp
+++ b/src/ackermann_trajectory_search.cpp
@@ -61,7 +61,7 @@ double AckermannTrajectorySearch::scoreTrajectory(base_local_planner::Trajectory
double cost = score_function_p->scoreTrajectory(traj);
if (cost < 0)
{
- ROS_INFO("Velocity %.3lf, %.3lf, %.3lf discarded by cost function %d with cost: %f", traj.xv_, traj.yv_, traj.thetav_, gen_id, cost);
+ ROS_DEBUG("Velocity %.3lf, %.3lf, %.3lf discarded by cost function %d with cost: %f", traj.xv_, traj.yv_, traj.thetav_, gen_id, cost);
traj_cost = cost;
break;
}
@@ -75,7 +75,7 @@ double AckermannTrajectorySearch::scoreTrajectory(base_local_planner::Trajectory
break;
}
gen_id ++;
- ROS_INFO("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);
+ 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);
}
return traj_cost;
@@ -104,7 +104,6 @@ bool AckermannTrajectorySearch::findBestTrajectory(base_local_planner::Trajector
count = 0;
count_valid = 0;
base_local_planner::TrajectorySampleGenerator* gen_ = *loop_gen;
- std::cout << "find trajectory" << std::endl;
while (gen_->hasMoreTrajectories())
{
gen_success = gen_->nextTrajectory(loop_traj);
@@ -114,7 +113,6 @@ bool AckermannTrajectorySearch::findBestTrajectory(base_local_planner::Trajector
continue;
}
loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
- std::cout << "cost: " << loop_traj_cost << std::endl;
if (all_explored != NULL)
{
loop_traj.cost_ = loop_traj_cost;
@@ -131,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)
{
--
GitLab