diff --git a/crosbot_explore/src/astar.cpp b/crosbot_explore/src/astar.cpp index 1eea9e911284111ac940317a6fe029927582aa84..43a5bd474bf42b625ec22ac767a0e59b1683de46 100644 --- a/crosbot_explore/src/astar.cpp +++ b/crosbot_explore/src/astar.cpp @@ -40,8 +40,7 @@ Plan PathPlanner::getPath(VoronoiGridPtr voronoi, Pose start, Pose goal, ImagePt start = trans * start.toTF(); goal = trans * goal.toTF(); -// LOG("Start location (%.3f, %.3f) and goal (%.3f, %.3f)\n", -// start.x, start.y, goal.x, goal.y); + ROS_INFO("PathPlanner::getPath() :: Start location (%.3f, %.3f) and goal (%.3f, %.3f)", start.position.x, start.position.y, goal.position.x, goal.position.y); startCell = Index2D(start.position.x / voronoi->resolution, start.position.y / voronoi->resolution); endCell = Index2D(goal.position.x / voronoi->resolution, goal.position.y / voronoi->resolution); @@ -71,7 +70,9 @@ Plan PathPlanner::getPath(VoronoiGridPtr voronoi, Pose start, Pose goal, ImagePt endCell.y = voronoi->height - 1; } + ROS_INFO("PathPlanner::getPath() :: before getCellPath"); CellPath cells = getCellPath(voronoi, startCell, endCell, image); + ROS_INFO("PathPlanner::getPath() :: after getCellPath"); // turn cells into points trans = voronoi->origin.toTF(); @@ -169,7 +170,16 @@ AStarParameters AStarPlanner::getParameters() { } double AStarPlanner::calculateCellRisk(const VoronoiGrid::VoronoiCell& cell, int maxExpandC, double expansionScale) { - double risk = astarParameters.k * (maxExpandC - cell.distanceFromWall) / expansionScale; + double risk = astarParameters.k; + //double risk = astarParameters.k * (maxExpandC - cell.distanceFromWall) / expansionScale; + + // TODO: If restricted, then scale by distance from wall ?? + if (cell.status == VoronoiGrid::VoronoiCell::Restricted || + cell.status == VoronoiGrid::VoronoiCell::PatiallyRestricted) { + risk = risk * (maxExpandC - cell.distanceFromWall) / expansionScale; + } + + switch (cell.status) { case VoronoiGrid::VoronoiCell::Wall: case VoronoiGrid::VoronoiCell::NotVisible: @@ -222,9 +232,9 @@ CellPath AStarPlanner::getCellPath(VoronoiGridPtr voronoi, Index2D startCell, In // Update goal cell // TODO: only pick new goal if parameters allow if (astarParameters.findTraversibleGoal) { - LOG("AStarPlanner::getPath(): finding new goalCell from (%d,%d)", goalCell.x, goalCell.y); + //LOG("AStarPlanner::getPath(): finding new goalCell from (%d,%d)", goalCell.x, goalCell.y); goalCell = rayTraceNewGoal(voronoi, startCell, goalCell); - LOG("AStarPlanner::getPath(): found new goalCell from (%d,%d)", goalCell.x, goalCell.y); + //LOG("AStarPlanner::getPath(): found new goalCell from (%d,%d)", goalCell.x, goalCell.y); } // Still ensure goal is traversible @@ -290,14 +300,16 @@ CellPath AStarPlanner::getCellPath(VoronoiGridPtr voronoi, Index2D startCell, In for (unsigned int n = 0; n < neighbours.size(); n++) { Index2D neighbour = node->currentCell + neighbours[n]; if (neighbour.x < 0 || neighbour.x >= (int)voronoi->width || - neighbour.y < 0 || neighbour.y >= (int)voronoi->height) + neighbour.y < 0 || neighbour.y >= (int)voronoi->height) { continue; + } // LOG("Neighbour of (%d, %d) is (%d, %d)\n", node->currentCell.x, node->currentCell.y, // neighbour.x, neighbour.y); int nc = neighbour.y*voronoi->width + neighbour.x; - if (cellStats[nc].closed) + if (cellStats[nc].closed) { continue; + } if (cellStats[nc].goalEstimate < 0) { cellStats[nc].goalEstimate = GOAL_ESTIMATE(neighbour); @@ -463,7 +475,7 @@ Index2D AStarPlanner::rayTraceNewGoal(VoronoiGridPtr voronoi, Index2D startCell, // Distance is start - goal, since going from goal to start dx = (startCell.x - goalCell.x) / (double) numRaySteps; dy = (startCell.y - goalCell.y) / (double) numRaySteps; - LOG("AStarPlanner::rayTraceNewGoal() startCell(%d,%d) rayd(%.2lf) nsteps(%d) dx(%.2lf) dy(%.2lf)", startCell.x, startCell.y, rayDistance, numRaySteps, dx, dy); + //LOG("AStarPlanner::rayTraceNewGoal() startCell(%d,%d) rayd(%.2lf) nsteps(%d) dx(%.2lf) dy(%.2lf)", startCell.x, startCell.y, rayDistance, numRaySteps, dx, dy); double x = goalCell.x; double y = goalCell.y; diff --git a/crosbot_explore/src/explorer.cpp b/crosbot_explore/src/explorer.cpp index b39ecb1bfac11294779d0981ba1a29d77c5fa9be..eb4ae2df41847bc1b917078b1d91b919fffeeab4 100644 --- a/crosbot_explore/src/explorer.cpp +++ b/crosbot_explore/src/explorer.cpp @@ -54,6 +54,7 @@ void Explorer::planThread() { Pose robot = getLatestPose(); if (voronoi != NULL && robot.isFinite()) { // search voronoi grid + ROS_INFO("%s find drive target", LOG_START); Pose foundTarget = findDriveTarget(*voronoi, robot); ROS_INFO("%s Drive target: %s", LOG_START, foundTarget.position.toString().c_str()); @@ -710,12 +711,14 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot) // Selected 'drive-to' target, default to no path Pose driveToTarget(INFINITY, INFINITY, INFINITY); + //ROS_INFO("%s Finding waypoint target, robot at: %s", LOG_START, robot.toString().c_str()); // Choose next waypoint to drive to {{ Lock lock(searchParams.waypointLock, true); // Find next waypoint based on distance to robot + // Note: Selected waypoint may be zero here, and waypoints empty int selectedWaypoint = searchParams.currentWaypoint; while (selectedWaypoint < searchParams.waypoints.size() && searchParams.waypoints[selectedWaypoint].position.distanceTo(robot.position) < searchParams.proximityDistance) { @@ -738,7 +741,6 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot) searchParams.currentWaypoint = selectedWaypoint; } }*/ - //ROS_INFO("%s selected point: %s dist: %.2lf, angle: %.1lf", LOG_START, searchParams.waypoints[searchParams.currentWaypoint].position.toString().c_str(), robot.position.distanceTo(searchParams.waypoints[searchParams.currentWaypoint].position), RAD2DEG(robot.headingToPointXY(searchParams.waypoints[searchParams.currentWaypoint].position))); // Check if "at destination" (and rotation desired) if (searchParams.waypoints.size() > 0) { @@ -773,6 +775,7 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot) // Otherwise update current waypoint target //ROS_INFO("%s too far from current waypoint - drive to it", LOG_START); searchParams.waypoint = searchParams.waypoints[searchParams.currentWaypoint]; + //ROS_INFO("%s selected point: %s dist: %.2lf, angle: %.1lf", LOG_START, searchParams.waypoints[searchParams.currentWaypoint].position.toString().c_str(), robot.position.distanceTo(searchParams.waypoints[searchParams.currentWaypoint].position), RAD2DEG(robot.headingToPointXY(searchParams.waypoints[searchParams.currentWaypoint].position))); } } else { // If no waypoints, just set everything to nil @@ -784,6 +787,8 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot) // If waypoint has been selected - find drive-able path to it if (!searchParams.atDestination && !searchParams.rotateToPose) { + //ROS_INFO("%s Find traversible path to %s", LOG_START, searchParams.waypoint.position.toString().c_str()); + // Head towards searchParams.waypoint double waypointHeading = atan2(searchParams.waypoint.position.y - robot.position.y, searchParams.waypoint.position.x - robot.position.x); double waypointDistance = searchParams.waypoint.position.distanceTo(robot.position); @@ -796,11 +801,12 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot) // Head directly towards waypoint, if far enough to searchParams.waypoint if (d >= minAcceptableDistance) { - //LOG("Explorer::findWaypointTarget(): Heading directly towards waypoint\n"); + //ROS_INFO("%s \t findWaypointTarget(): Heading directly towards waypoint, heading: %.2lf, dist: %.2lf", LOG_START, waypointHeading, waypointDistance); searchParams.pathBlocked = false; Point target = robot.position + Point(d*cos(waypointHeading), d*sin(waypointHeading), 0); driveToTarget = Pose(target, Quaternion()); } else { + ROS_INFO("%s \t findWaypointTarget(): Finding path in direction of waypoint", LOG_START); // Otherwise search for a path in the direction of the waypoint, at increasing radius of angles bool haveTarget = false; double a = searchParams.angleStep; diff --git a/crosbot_explore/src/explorerROS.cpp b/crosbot_explore/src/explorerROS.cpp index 5324b68a6b19787ae2f6573c7dc272f0c6c6207d..5c21d4ac1abc280857f279e434ad24685da308e5 100644 --- a/crosbot_explore/src/explorerROS.cpp +++ b/crosbot_explore/src/explorerROS.cpp @@ -171,12 +171,16 @@ void ExplorerROSNode::configure() { // Configure base class Explorer::configure(); - // read configuration/parameters + // Read configuration/parameters ros::NodeHandle paramNH("~"); - paramNH.param("base_frame", baseFrame, DEFAULT_BASEFRAME); - ConfigElementPtr cfg = new ROSConfigElement(paramNH); + + // Explorer class config debugMsgs = cfg->getParamAsBool("debug", false); + searchWhilePaused = cfg->getParamAsBool("searchWhilePaused", true); + + // ROS Config + baseFrame = cfg->getParam("base_frame", DEFAULT_BASEFRAME); explorer_srv_name = cfg->getParam("explorer_srv", "/crosbot_explore/explorer_service"); explorer_feedback_pub_name = cfg->getParam("explorer_feedback_pub", "/crosbot_explore/explorer_feedback"); rate_driveThread = cfg->getParamAsDouble("rate_driveThread", 10); @@ -223,6 +227,7 @@ void ExplorerROSNode::configure() { hysteresisParams.marginHeading = hysteresisCfg->getParamAsDouble("margin_heading", hysteresisParams.marginHeading); } + // Advertise ROS subscribers and publishers ros::NodeHandle nh; subscribeGrid(nh); explorer_srv = nh.advertiseService(explorer_srv_name, &ExplorerROSNode::callbackExplorerMode, this); @@ -239,6 +244,7 @@ void ExplorerROSNode::configure() { reconfig_server->setCallback(reconfig_callbackType); ROS_INFO("%s configure done", LOG_START); + ROS_INFO("%s \t %d", LOG_START, searchWhilePaused); } void ExplorerROSNode::startup() { @@ -464,8 +470,11 @@ bool ExplorerROSNode::setWaypoints(nav_msgs::Path& navPath) { tf::StampedTransform transform; try { ros::Time now = ros::Time::now(); + ROS_INFO("%s Setting transform from: %s to %s", LOG_START, voronoi->frame.c_str(), navPath.header.frame_id.c_str()); tfListener.waitForTransform(voronoi->frame, navPath.header.frame_id, now, ros::Duration(DEFAULT_MAXWAIT4TRANSFORM)); tfListener.lookupTransform(voronoi->frame, navPath.header.frame_id, now, transform); + + ROS_INFO("%s Transform is: %s", LOG_START, crosbot::Pose3D(transform.getOrigin(), transform.getRotation()).toString().c_str()); } catch (tf::TransformException& ex) { ROS_ERROR("%s: Error getting transform for voronoi frame to navPath. (%s)\n", LOG_START, ex.what()); return false; @@ -476,11 +485,14 @@ bool ExplorerROSNode::setWaypoints(nav_msgs::Path& navPath) { waypoints.resize(navPath.poses.size()); for (size_t i = 0; i < waypoints.size(); ++i) { waypoints[i] = transform * Pose(navPath.poses[i]).toTF(); + //ROS_INFO("%s Adding waypoint %s", LOG_START, waypoints[i].position.toString().c_str()); } } // Set waypoints - searchParams.setWaypoints(waypoints, getLatestPose()); + crosbot::Pose3D currentPose = getLatestPose(); + //ROS_INFO("%s Current pose: %s", LOG_START, currentPose.toString().c_str()); + searchParams.setWaypoints(waypoints, currentPose); return true; } diff --git a/crosbot_explore/src/nodes/astar_explorer_node.cpp b/crosbot_explore/src/nodes/astar_explorer_node.cpp index b2a658746cee18f852f8e5a1f73c999607cc6644..7fbf07e5d7ed4efea5c96d3bfc1b2ecbaac9abe2 100644 --- a/crosbot_explore/src/nodes/astar_explorer_node.cpp +++ b/crosbot_explore/src/nodes/astar_explorer_node.cpp @@ -190,8 +190,6 @@ VoronoiGridPtr AStarExplorerROSNode::getLatestAstarVoronoi() { VoronoiGridPtr voronoi = NULL; if (differentPlanningGrid) { - // TODO: put back? - ROS_ERROR("%s different planning grid is disabled", LOG_START); voronoi = astarLatestVoronoi; } else { voronoi = getLatestVoronoi(); @@ -306,6 +304,7 @@ bool AStarExplorerROSNode::setWaypoints(Plan& plan, const std::string& planFrame // Generate navPath if (converted) { + ROS_INFO("%s Sending path in frame: %s", LOG_START, convertedFrameId.c_str()); nav_msgs::Path navPath; navPath.header.frame_id = convertedFrameId; navPath.header.stamp = time.toROS(); @@ -392,6 +391,11 @@ bool AStarExplorerROSNode::updateWaypoints() { debugImage = localVoronoi->getImage(); } + // Only loaded if there are waypoints calculated - voronoi could be loaded in between high-level calculation + if (astarWaypoints.size() == 0) { + loaded = false; + } + bool waypointsSet = false; if (loaded) { // Iterate through A* waypoints - removing any that are too close - but keep goal @@ -405,7 +409,14 @@ bool AStarExplorerROSNode::updateWaypoints() { // Plan to set planner.astarParameters.findTraversibleGoal = false; Plan plan = planner.getPath(localVoronoi, robot, goal, debugImage); - if (debugMsgs) ROS_INFO("%s Loaded path with %d waypoints to %s", LOG_START, (int) plan.size(), goal.position.toString().c_str()); + if (debugMsgs) { + ROS_INFO("%s Loaded path with %d waypoints to %s", LOG_START, (int) plan.size(), goal.position.toString().c_str()); +// int i = 0; +// for (auto p : plan) { +// ROS_INFO("%s \t %d to %s", LOG_START, i, p.position.toString().c_str()); +// ++i; +// } + } // Append remaining waypoints to path (to stop reaching goal) for (int i = 1; i < astarWaypoints.size(); ++i) { @@ -447,7 +458,6 @@ void AStarExplorerROSNode::calculateAStarPath() { loadLatestPose(); robot = getLatestPose(); - // TODO: put astarvoronoi back astarVoronoi = getLatestAstarVoronoi(); // Error checking