bool DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { // dispatches to either dwa sampling control or stop and rotate control, depending on whether we have been close enough to goal if ( ! costmap_ros_->getRobotPose(current_pose_)) { ROS_ERROR("Could not get robot pose"); return false; } std::vector<geometry_msgs::PoseStamped> transformed_plan; if ( ! planner_util_.getLocalPlan(current_pose_, transformed_plan)) { ROS_ERROR("Could not get local plan"); return false; } //if the global plan passed in is empty... we won't do anything if(transformed_plan.empty()) { ROS_WARN_NAMED("dwa_local_planner", "Received an empty transformed plan."); return false; } ROS_DEBUG_NAMED("dwa_local_planner", "Received a transformed plan with %zu points.", transformed_plan.size()); //we want to clear the robot footprint from the costmap we're using costmap_ros_->clearRobotFootprint(); // make sure to update the costmap we'll use for this cycle costmap_ros_->getCostmapCopy(costmap_); // update plan in dwa_planner even if we just stop and rotate, to allow checkTrajectory dp_->updatePlanAndLocalCosts(current_pose_, transformed_plan); if (latchedStopRotateController_.isPositionReached(&planner_util_, current_pose_)) { //publish an empty plan because we've reached our goal position std::vector<geometry_msgs::PoseStamped> local_plan; std::vector<geometry_msgs::PoseStamped> transformed_plan; publishGlobalPlan(transformed_plan); publishLocalPlan(local_plan); base_local_planner::LocalPlannerLimits limits = planner_util_.getCurrentLimits(); return latchedStopRotateController_.computeVelocityCommandsStopRotate( cmd_vel, limits.getAccLimits(), dp_->getSimPeriod(), &planner_util_, odom_helper_, current_pose_, boost::bind(&DWAPlanner::checkTrajectory, dp_, _1, _2, _3)); } else { bool isOk = dwaComputeVelocityCommands(current_pose_, cmd_vel); if (isOk) { publishGlobalPlan(transformed_plan); } else { ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path."); std::vector<geometry_msgs::PoseStamped> empty_plan; publishGlobalPlan(empty_plan); } return isOk; } }
bool DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped<tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel) { // dynamic window sampling approach to get useful velocity commands if(! isInitialized()){ ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner"); return false; } tf::Stamped<tf::Pose> robot_vel; odom_helper_.getRobotVel(robot_vel); /* For timing uncomment struct timeval start, end; double start_t, end_t, t_diff; gettimeofday(&start, NULL); */ //compute what trajectory to drive along tf::Stamped<tf::Pose> drive_cmds; drive_cmds.frame_id_ = costmap_ros_->getBaseFrameID(); // call with updated footprint base_local_planner::Trajectory path = dp_->findBestPath(global_pose, robot_vel, drive_cmds, costmap_ros_->getRobotFootprint()); //ROS_ERROR("Best: %.2f, %.2f, %.2f, %.2f", path.xv_, path.yv_, path.thetav_, path.cost_); /* For timing uncomment gettimeofday(&end, NULL); start_t = start.tv_sec + double(start.tv_usec) / 1e6; end_t = end.tv_sec + double(end.tv_usec) / 1e6; t_diff = end_t - start_t; ROS_INFO("Cycle time: %.9f", t_diff); */ //pass along drive commands cmd_vel.linear.x = drive_cmds.getOrigin().getX(); cmd_vel.linear.y = drive_cmds.getOrigin().getY(); cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation()); //if we cannot move... tell someone std::vector<geometry_msgs::PoseStamped> local_plan; if(path.cost_ < 0) { ROS_DEBUG_NAMED("dwa_local_planner", "The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot."); local_plan.clear(); publishLocalPlan(local_plan); return false; } ROS_DEBUG_NAMED("dwa_local_planner", "A valid velocity command of (%.2f, %.2f, %.2f) was found for this cycle.", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z); // Fill out the local plan for(unsigned int i = 0; i < path.getPointsSize(); ++i) { double p_x, p_y, p_th; path.getPoint(i, p_x, p_y, p_th); tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose( tf::createQuaternionFromYaw(p_th), tf::Point(p_x, p_y, 0.0)), ros::Time::now(), costmap_ros_->getGlobalFrameID()); geometry_msgs::PoseStamped pose; tf::poseStampedTFToMsg(p, pose); local_plan.push_back(pose); } //publish information to the visualizer publishLocalPlan(local_plan); return true; }