diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp
index 9c43ed8f637f2f1459387bcd0d0de8866019d0fc..20648ef40d74427d42873c69c2bb94cddcd8346f 100644
--- a/src/ackermann_planner_ros.cpp
+++ b/src/ackermann_planner_ros.cpp
@@ -148,14 +148,14 @@ void AckermannPlannerROS::initialize(std::string name,tf2_ros::Buffer* tf,costma
}
else
{
- ROS_WARN("This planner has already been initialized, doing nothing.");
+ ROS_WARN("AckermannPlannerROS: this planner has already been initialized, doing nothing.");
}
}
bool AckermannPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if (! is_initialized())
{
- ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
+ ROS_ERROR("AckermannPlannerROS: this planner has not been initialized, please call initialize() before using this planner");
return false;
}
//when we get a new plan, we also want to clear any latch we may have on goal tolerances
@@ -184,7 +184,7 @@ bool AckermannPlannerROS::ackermann_compute_velocity_commands(geometry_msgs::Pos
// dynamic window sampling approach to get useful velocity commands
if(! is_initialized())
{
- ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
+ ROS_ERROR("AckermannPlannerROS: this planner has not been initialized, please call initialize() before using this planner");
return false;
}
@@ -275,17 +275,17 @@ bool AckermannPlannerROS::isGoalReached(void)
AckermannPlannerLimits limits;
if (! is_initialized()) {
- ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
+ ROS_ERROR("AckermannPlannerROS: this planner has not been initialized, please call initialize() before using this planner");
return false;
}
if ( ! costmap_ros_->getRobotPose(current_pose_)) {
- ROS_ERROR("Could not get robot pose");
+ ROS_ERROR("AckermannPlannerROS: could not get robot pose");
return false;
}
std::vector<geometry_msgs::PoseStamped> transformed_plan;
if ( ! planner_util_.get_local_plan(current_pose_, transformed_plan))
{
- ROS_ERROR("Could not get local plan");
+ ROS_ERROR("AckermannPlannerROS: could not get local plan");
return false;
}
odom_helper_.get_odom(odom);
@@ -301,7 +301,7 @@ bool AckermannPlannerROS::isGoalReached(void)
limits.rot_stopped_vel,limits.trans_stopped_vel,
limits.xy_goal_tolerance,limits.yaw_goal_tolerance))
{
- ROS_INFO("Goal reached");
+ ROS_INFO("AckermannPlannerROS: Goal reached");
return true;
}
else if(this->stucked)
@@ -366,7 +366,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
std::vector<geometry_msgs::PoseStamped> transformed_plan;
if ( ! planner_util_.get_local_plan(current_pose_, transformed_plan))
{
- ROS_ERROR("Could not get local plan");
+ ROS_ERROR("AckermannPlannerROS: could not get local plan");
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
@@ -449,9 +449,12 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
// steer the car without moving if the steering angle is big
if(this->use_steer_angle_cmd && this->stopped_steering)
{
- if(fabs(ackermann.steering_angle-cmd_vel.angular.z)>0.05)
+ double ang_z = cmd_vel.angular.z;
+ if(this->invert_steer_angle_cmd)
+ ang_z = -ang_z;
+ if(fabs(ackermann.steering_angle-ang_z)>0.05)
{
- ROS_WARN("Setting forward speed to 0 (%f,%f)",ackermann.steering_angle,cmd_vel.angular.z);
+ //ROS_WARN("AckermannPlannerROS: steering without moving. Setting forward speed to 0 (%f,%f)",ackermann.steering_angle,cmd_vel.angular.z);
cmd_vel.linear.x=0.0;
}
else
@@ -472,7 +475,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
}
else if(fabs(cmd_vel.linear.x)<limits.trans_vel_deadzone)
{
-/*
+ //ROS_INFO_STREAM("linear.x " << cmd_vel.linear.x << " deadzone: " << limits.trans_vel_deadzone );
// get the position of the goal
base_local_planner::getGoalPose(*tf_,
transformed_plan,
@@ -486,7 +489,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
if(stucked_count>10)
{
this->stucked=true;
- ROS_INFO("Robot is stucked, jumping to the next segment");
+ ROS_INFO("AckermannPlannerROS: Robot may be stucked near segment end point. Jumping to the next path segment");
}
}
else
@@ -494,7 +497,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
replan_count++;
if(replan_count>10)
{
- ROS_INFO("Replan because robot can not move");
+ ROS_INFO("AckermannPlannerROS: Robot seems stucked. Replanning");
cmd_vel.linear.x = 0.0;
cmd_vel.linear.y = 0.0;
cmd_vel.angular.z = 0.0;
@@ -505,13 +508,14 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
return false;
}
}
-*/
+
}
else
{
stucked_count=0;
replan_count=0;
}
+
if (isOk)
{
publish_global_plan(transformed_plan);
@@ -519,7 +523,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
if(cmd_vel.linear.x>0.0)
cmd_vel.linear.x=limits.trans_vel_deadzone;
- else
+ else if(cmd_vel.linear.x<0.0)
cmd_vel.linear.x=-limits.trans_vel_deadzone;
}
}
@@ -539,6 +543,9 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
return isOk;
}
+
+ //otherwise, return false
+ return false;
}
AckermannPlannerROS::~AckermannPlannerROS()
diff --git a/src/ackermann_planner_util.cpp b/src/ackermann_planner_util.cpp
index 80f11597c47110bbd11fbd1e7f48c25076771202..11254acddd7b05a3076e7eef7445407690fc01cc 100644
--- a/src/ackermann_planner_util.cpp
+++ b/src/ackermann_planner_util.cpp
@@ -55,7 +55,7 @@ void AckermannPlannerUtil::initialize(tf2_ros::Buffer* tf, costmap_2d::Costmap2D
}
else
{
- ROS_WARN("Planner utils have already been initialized, doing nothing.");
+ ROS_WARN("AckermannPlannerUtil: planner utils have already been initialized, doing nothing.");
}
}
@@ -98,7 +98,7 @@ bool AckermannPlannerUtil::get_goal(geometry_msgs::PoseStamped& goal_pose)
bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {
if(!initialized_)
{
- ROS_ERROR("Planner utils have not been initialized, please call initialize() first");
+ ROS_ERROR("AckermannPlannerUtil: planner utils have not been initialized, please call initialize() first");
return false;
}
////divide plan by manuveurs
@@ -139,12 +139,12 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
{
if(pathLength>limits_.split_ignore_length)
{
- ROS_INFO("TrajectoryPlannerROS::setPlan: subPath split at i=%d, angle=%f, length=%f", i, angle, pathLength);
+ //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, length=%f", i, angle, pathLength);
subPathList.push_back(subPath);
}
- else //ignored subpaths shorter than 1.0m
+ else //ignored subpath, too short
{
- ROS_INFO("TrajectoryPlannerROS::setPlan: subPath split at i=%d, angle=%f, Ignored by length=%f", i, angle, pathLength);
+ //ROS_INFO("AckermannPlannerUtil::setPlan: subPath split at i=%d, angle=%f, Ignored by length=%f", i, angle, pathLength);
}
subPath.clear();
pathLength=0.0;
@@ -154,8 +154,8 @@ bool AckermannPlannerUtil::set_plan(const std::vector<geometry_msgs::PoseStamped
subPath.push_back(orig_global_plan[i]);
}
subPathList.push_back(subPath);
- ROS_INFO("TrajectoryPlannerROS::setPlan: subPath last length=%f", pathLength);
- ROS_INFO("TrajectoryPlannerROS::setPlan: Global plan (%lu points) split in %lu paths", orig_global_plan.size(), subPathList.size());
+ //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());
//reset the global plan
global_plan_.clear();
@@ -194,7 +194,7 @@ bool AckermannPlannerUtil::get_local_plan(geometry_msgs::PoseStamped& global_pos
//get the global plan in our frame
if(!base_local_planner::transformGlobalPlan(*tf_,global_plan_,global_pose,*costmap_,global_frame_,transformed_plan)) {
- ROS_WARN("Could not transform the global plan to the frame of the controller");
+ ROS_WARN("AckermannPlannerUtil: could not transform the global plan to the frame of the controller");
return false;
}