diff --git a/include/ackermann_planner.h b/include/ackermann_planner.h
index f07e6b94bc18802325b9c83af979e38d18975270..496f78492286f4f7847f74b02851d35bd1c0eef7 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 862cc76015883d886839c803fae87e281f6584e3..1025232b205b392982da94176dd23ea993e4578c 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 e30b934269ee4b0a9631f4ba13acb04787737bb3..46d1c3e744136e47fd7aafe1de70115a4f1ffaf2 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 8bcada87d4dac4d970221bb2a71f05ec75fcdf9a..dc83b217534dcc57fbc5b9c5fc2abda6e98909e4 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 6f8c48851a56a73d96aab8d602d1e48f11c5e099..8ebd7d5d4822f17d29acd6d1cee80c7cf42941c9 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 a9f19098a517e385de8bad40a695c7ae7ef5d552..d79cd196bd926dbd73ed5e1fd87bd19fe130e2cd 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 0867043113a8b3d6ad762718625a623a54174a17..c29da3bd870ddb5094c1a566238216d06c205110 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 c295941f9db6e5b228530dc5a97568473e556c33..85a77ca0ceddc185a4997864b8e0bdae3d1edb8c 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 0da364348986de436a760fc57cd339f4ca6e89f2..7ba497220a48054eab38d3c50e072c3485a6e02e 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)
{