Skip to content
Snippets Groups Projects
Commit a8d4923c authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Uncomment stucked check behavior. Rename ROSout texts

parent cf0b6dd8
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/ackermann_planner_ros.cpp
+ 22
− 15
View file @ a8d4923c
......@@ -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()
......
src/ackermann_planner_util.cpp
+ 8
− 8
View file @ a8d4923c
......@@ -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;
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment