/** * @brief A point of the road is visible if it is between the robot and the laser beam running through it, and if the previous point was visible * All points in the road are updated * @param road ... * @param laserData ... * @return bool */ bool ElasticBand::checkVisiblePoints(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData) { //Simplify laser polyline using Ramer-Douglas-Peucker algorithm std::vector<Point> points, res; QVec wd; for (auto &ld : laserData) { //wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS wd = innermodel->getNode<InnerModelLaser>("laser")->laserTo("world", ld.dist, ld.angle); points.push_back(Point(wd.x(), wd.z())); } res = simPath.simplifyWithRDP(points, 70); //qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size(); // Create a QPolygon so we can check if robot outline falls inside QPolygonF polygon; for (auto &p: res) polygon << QPointF(p.x, p.y); // Move the robot along the road int robot = road.getIndexOfNextPoint(); QVec memo = innermodel->transform6D("world", "robot"); for(int it = robot; it<road.size(); ++it) { road[it].isVisible = true; innermodel->updateTransformValues("robot", road[it].pos.x(), road[it].pos.y(), road[it].pos.z(), 0, road[it].rot.y(), 0); //get Robot transformation matrix QMat m = innermodel->getTransformationMatrix("world", "robot"); // Transform all points at one to world RS //m.print("m"); //pointsMat.print("pointsMat"); QMat newPoints = m * pointsMat; //Check if they are inside the laser polygon for (int i = 0; i < newPoints.nCols(); i++) { // qDebug() << __FUNCTION__ << "----------------------------------"; // qDebug() << __FUNCTION__ << QPointF(newPoints(0, i), newPoints(2, i)); // qDebug() << __FUNCTION__ << polygon; if (polygon.containsPoint(QPointF(newPoints(0, i), newPoints(2, i)),Qt::OddEvenFill) == false) { road[it].isVisible = false; //qFatal("fary"); break; } } // if( road[it].isVisible == false) // { // for (int k = it; k < road.size(); ++k) // road[k].isVisible = false; // break; // } } // Set the robot back to its original state innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0); //road.print(); return true; }
static int getNextWaypoint() { int path_size = static_cast<int>(_current_waypoints.getSize()); double lookahead_threshold = getLookAheadThreshold(0); // if waypoints are not given, do nothing. if (_current_waypoints.isEmpty()) { return -1; } // look for the next waypoint. for(int i = 0; i < path_size; i++) { //if search waypoint is the last if (i == (path_size - 1)) { ROS_INFO("search waypoint is the last"); return i; } // if there exists an effective waypoint if (getPlaneDistance(_current_waypoints.getWaypointPosition(i), _current_pose.pose.position) > lookahead_threshold) { return i; } } //if this program reaches here , it means we lost the waypoint! return -1; }
static void ChangeWaypoint(EControl detection_result) { int obs = _obstacle_waypoint; if (obs != -1){ std::cout << "====got obstacle waypoint====" << std::endl; std::cout << "=============================" << std::endl; } if (detection_result == STOP){ // STOP for obstacle // stop_waypoint is about _others_distance meter away from obstacles int stop_waypoint = obs - ((int)(_others_distance / _path_change.getInterval())); std::cout << "stop_waypoint: " << stop_waypoint << std::endl; // change waypoints to stop by the stop_waypoint _path_change.changeWaypoints(stop_waypoint); _path_change.avoidSuddenBraking(); _path_change.setTemporalWaypoints(); _temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints()); } else if (detection_result == DECELERATE) { // DECELERATE for obstacles _path_change.setPath(_path_dk.getCurrentWaypoints()); _path_change.setDeceleration(); _path_change.setTemporalWaypoints(); _temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints()); } else { // ACELERATE or KEEP _path_change.setPath(_path_dk.getCurrentWaypoints()); _path_change.avoidSuddenAceleration(); _path_change.avoidSuddenBraking(); _path_change.setTemporalWaypoints(); _temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints()); } return; }
/** * @brief Check if any of the waypoints has nan coordinates * * @param road ... * @return bool */ bool ElasticBand::checkIfNAN(const WayPoints &road) { for (auto it = road.begin(); it != road.end(); ++it) if (std::isnan(it->pos.x()) or std::isnan(it->pos.y()) or std::isnan(it->pos.z())) { road.print(); return true; } return false; }
static void doPurePursuit() { //search next waypoint int next_waypoint = getNextWaypoint(); displayNextWaypoint(next_waypoint); displaySearchRadius(getLookAheadThreshold(0)); //ROS_INFO_STREAM("next waypoint = " << next_waypoint << "/" << path_size - 1); //linear interpolation and calculate angular velocity geometry_msgs::Point next_target; geometry_msgs::TwistStamped twist; std_msgs::Bool wf_stat; bool interpolate_flag = false; if (!g_linear_interpolate_mode) { next_target = _current_waypoints.getWaypointPosition(next_waypoint); } else { interpolate_flag = interpolateNextTarget(next_waypoint, &next_target); } if (g_linear_interpolate_mode && !interpolate_flag) { ROS_INFO_STREAM("lost target! "); wf_stat.data = false; _stat_pub.publish(wf_stat); twist.twist.linear.x = 0; twist.twist.angular.z = 0; twist.header.stamp = ros::Time::now(); g_cmd_velocity_publisher.publish(twist); return; } //ROS_INFO("next_target : ( %lf , %lf , %lf)", next_target.x, next_target.y,next_target.z); displayNextTarget(next_target); displayTrajectoryCircle(generateTrajectoryCircle(next_target)); twist.twist = calcTwist(calcCurvature(next_target), getCmdVelocity(0)); wf_stat.data = true; _stat_pub.publish(wf_stat); twist.header.stamp = ros::Time::now(); g_cmd_velocity_publisher.publish(twist); //ROS_INFO("linear : %lf, angular : %lf",twist.twist.linear.x,twist.twist.angular.z); #ifdef LOG std::ofstream ofs("/tmp/pure_pursuit.log", std::ios::app); ofs << _current_waypoints.getWaypointPosition(next_waypoint).x << " " << _current_waypoints.getWaypointPosition(next_waypoint).y << " " << next_target.x << " " << next_target.y << std::endl; #endif }
static int FindCrossWalk() { if (!vmap.set_points || _closest_waypoint < 0) return -1; double find_distance = 2.0*2.0; // meter double ignore_distance = 20.0*20.0; // meter // Find near cross walk for (int num = _closest_waypoint; num < _closest_waypoint+_search_distance; num++) { geometry_msgs::Point waypoint = _path_dk.getWaypointPosition(num); waypoint.z = 0.0; // ignore Z axis for (int i = 1; i < vmap.getSize(); i++) { // ignore far crosswalk geometry_msgs::Point crosswalk_center = vmap.getDetectionPoints(i).center; crosswalk_center.z = 0.0; if (CalcSquareOfLength(crosswalk_center, waypoint) > ignore_distance) continue; for (auto p : vmap.getDetectionPoints(i).points) { p.z = waypoint.z; if (CalcSquareOfLength(p, waypoint) < find_distance) { vmap.setDetectionCrossWalkID(i); return num; } } } } vmap.setDetectionCrossWalkID(-1); return -1; // no near crosswalk }
/** * @brief Stops the robot * * @return bool */ bool SpecificWorker::stopCommand(CurrentTarget &target, WayPoints &myRoad, TrajectoryState &state) { controller->stopTheRobot(omnirobot_proxy); myRoad.setFinished(true); myRoad.reset(); myRoad.endRoad(); #ifdef USE_QTGUI //myRoad.clearDraw(viewer->innerViewer); //drawGreenBoxOnTarget(target.getTranslation()); #endif target.reset(); state.setElapsedTime(taskReloj.elapsed()); state.setState("IDLE"); qDebug() << __FUNCTION__ << "Robot at pose:" << innerModel->transform6D("world", "robot"); return true; }
static bool verifyFollowing() { double slope = 0; double intercept = 0; getLinearEquation(_current_waypoints.getWaypointPosition(1),_current_waypoints.getWaypointPosition(2),&slope,&intercept); double displacement = getDistanceBetweenLineAndPoint(_current_pose.pose.position,slope,intercept); double relative_angle = getRelativeAngle(_current_waypoints.getWaypointPose(1),_current_pose.pose); //ROS_INFO("side diff : %lf , angle diff : %lf",displacement,relative_angle); if(displacement < g_displacement_threshold || relative_angle < g_relative_angle_threshold){ //ROS_INFO("Following : True"); return true; } else{ //ROS_INFO("Following : False"); return false; } }
/** * @brief Adds points to the band if two existing ones are too far apart (ROBOT_RADIUS) * * @param road ... * @return void */ void ElasticBand::addPoints(WayPoints &road, const CurrentTarget ¤tTarget) { int offset = 1; for (int i = 0; i < road.size() - offset; i++) { if (i > 0 and road[i].isVisible == false) break; WayPoint &w = road[i]; WayPoint &wNext = road[i + 1]; float dist = (w.pos - wNext.pos).norm2(); if (dist > ROAD_STEP_SEPARATION) //SHOULD GET FROM IM { float l = 0.9 * ROAD_STEP_SEPARATION / dist; //Crucial que el punto se ponga mas cerca que la condición de entrada WayPoint wNew((w.pos * (1 - l)) + (wNext.pos * l)); road.insert(i + 1, wNew); } } //ELIMINATED AS REQUESTED BY MANSO //Move point before last to orient the robot. This works but only if the robots approaches from the lower quadrants //The angle formed by this point and the last one has to be the same es specified in the target //We solve this equations for (x,z) // (x' -x)/(z'-z) = tg(a) = t // sqr(x'-x) + sqr(z'-z) = sqr(r) // z = z' - (r/(sqrt(t*t -1))) // x = x' - r(sqrt(1-(1/t*t+1))) // if( (currentTarget.hasRotation() == true) and (road.last().hasRotation == false) ) // { // qDebug() << __FUNCTION__ << "computing rotation" << road.last().pos; // float radius = 500; // float ta = tan(currentTarget.getRotation().y()); // float xx = road.last().pos.x() - radius*sqrt(1.f - (1.f/(ta*ta+1))); // float zz = road.last().pos.z() - (radius/sqrt(ta*ta+1)); // WayPoint wNew( QVec::vec3(xx,road.last().pos.y(),zz) ); // road.insert(road.end()-1,wNew); // road.last().hasRotation = true; // qDebug() << __FUNCTION__ << "after rotation" << wNew.pos << currentTarget.getRotation().y() << ta; // // } //else //qDebug() << road.last().hasRotation << road.last().pos << (road.end()-2)->pos << currentTarget.getRotation().y(); }
/** * @brief Check if some point ahead on the road is closer (L2) than along the road, to take a shortcut * * @param innermodel ... * @param road ... * @param laserData ... * @return bool */ bool ElasticBand::shortCut(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData) { //Compute distances from robot to all points ahead. If any of them is laser-visible and significantly shorter than de distance along de road, try it! WayPoints::iterator robot = road.getIterToClosestPointToRobot(); WayPoints::iterator best = road.begin(); for (WayPoints::iterator it = robot + 1; it != road.end(); ++it) { //qDebug() << __FUNCTION__ << it->isVisible << (it->pos - robot->pos).norm2() << road.computeDistanceBetweenPointsAlongRoad(robot, it); if ( it->isVisible ) { if (road.computeDistanceBetweenPointsAlongRoad(robot, it) - (it->pos - robot->pos).norm2() > 300) //Half robot SACARRRR { qDebug() << __FUNCTION__ << "Candidato"; //Check if the robot passes through the straight line if (checkCollisionAlongRoad(innermodel, laserData, road, robot, it, ROBOT_RADIUS)) { //Is so remove all intermadiate points between robot and new subtarget qDebug() << __FUNCTION__ << "Confirmado"; best = it; } } } else break; } if (best != road.begin() and (robot + 1) != road.end()) road.erase(robot + 1, best); return false; }
static double getCmdVelocity(int waypoint) { if (_param_flag == MODE_DIALOG) { ROS_INFO_STREAM("dialog : " << _initial_velocity << " km/h (" << kmph2mps(_initial_velocity) << " m/s )"); return kmph2mps(_initial_velocity); } if (_current_waypoints.isEmpty()) { ROS_INFO_STREAM("waypoint : not loaded path"); return 0; } double velocity = _current_waypoints.getWaypointVelocityMPS(waypoint); //ROS_INFO_STREAM("waypoint : " << mps2kmph(velocity) << " km/h ( " << velocity << "m/s )"); return velocity; }
void BaseWaypointCallback(const waypoint_follower::laneConstPtr &msg) { ROS_INFO("subscribed base_waypoint\n"); _path_dk.setPath(*msg); _path_change.setPath(*msg); if (_path_flag == false) { std::cout << "waypoint subscribed" << std::endl; _path_flag = true; } }
/** * @brief Removes points from the band if two of them are too close, ROBOT_RADIUS/3. * * @param road ... * @return void */ void ElasticBand::cleanPoints(WayPoints &road) { int i; int offset = 2; //if( road.last().hasRotation ) offset = 3; else offset = 2; for (i = 1; i < road.size() - offset; i++) // exlude 1 to avoid deleting the nextPoint and the last two to avoid deleting the target rotation { if (road[i].isVisible == false) break; WayPoint &w = road[i]; WayPoint &wNext = road[i + 1]; float dist = (w.pos - wNext.pos).norm2(); if (dist < ROAD_STEP_SEPARATION / 3.) { road.removeAt(i + 1); } } }
bool ElasticBand::update(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData, const CurrentTarget ¤tTarget, uint iter) { //qDebug() << __FILE__ << __FUNCTION__ << "road size"<< road.size(); if (road.isFinished() == true) return false; ///////////////////////////////////////////// //Tags all points in the road ar visible or blocked, depending on laser visibility. Only visible points are processed in this iteration ///////////////////////////////////////////// //checkVisiblePoints(innermodel, road, laserData); ///////////////////////////////////////////// //Check if there is a sudden shortcut to take ///////////////////////////////////////////// //shortCut(innermodel, road, laserData); ///////////////////////////////////////////// //Add points to achieve an homogenoeus chain ///////////////////////////////////////////// addPoints(road, currentTarget); ///////////////////////////////////////////// //Remove point too close to each other ///////////////////////////////////////////// cleanPoints(road); ///////////////////////////////////////////// //Compute the scalar magnitudes ///////////////////////////////////////////// computeForces(innermodel, road, laserData); ///////////////////////////////////////////// //Delete half the tail behind, if greater than 6, to release resources ///////////////////////////////////////////// if (road.getIndexOfClosestPointToRobot() > 6) { for (auto it = road.begin(); it != road.begin() + (road.getIndexOfCurrentPoint() / 2); ++it) road.backList.append(it->pos); road.erase(road.begin(), road.begin() + (road.getIndexOfCurrentPoint() / 2)); } return true; }
void PathVset::changeWaypoints(int stop_waypoint) { int i = 0; int close_waypoint_threshold = 4; int fill_in_zero = 20; double changed_vel; double interval = getInterval(); // change waypoints to decelerate for (int num = stop_waypoint; num > _closest_waypoint - close_waypoint_threshold; num--){ if (!checkWaypoint(num, "changeWaypoints")) continue; changed_vel = sqrt(2.0*_decel*(interval*i)); // sqrt(2*a*x) //std::cout << "changed_vel[" << num << "]: " << mps2kmph(changed_vel) << " (km/h)"; //std::cout << " distance: " << (_obstacle_waypoint-num)*interval << " (m)"; //std::cout << " current_vel: " << mps2kmph(_current_vel) << std::endl; waypoint_follower::waypoint initial_waypoint = _path_dk.getCurrentWaypoints().waypoints[num]; if (changed_vel > _velocity_limit || // changed_vel > initial_waypoint.twist.twist.linear.x){ // avoid acceleration //std::cout << "too large velocity!!" << std::endl; current_waypoints_.waypoints[num].twist.twist.linear.x = initial_waypoint.twist.twist.linear.x; } else { current_waypoints_.waypoints[num].twist.twist.linear.x = changed_vel; } i++; } // fill in 0 for (int j = 1; j < fill_in_zero; j++){ if (!checkWaypoint(stop_waypoint+j, "changeWaypoints")) continue; current_waypoints_.waypoints[stop_waypoint+j].twist.twist.linear.x = 0.0; } std::cout << "---changed waypoints---" << std::endl; return; }
// display the next waypoint by markers. static void displayNextWaypoint(int i) { visualization_msgs::Marker marker; marker.header.frame_id = MAP_FRAME; marker.header.stamp = ros::Time(); marker.ns = "next_waypoint_marker"; marker.id = 0; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.pose.position = _current_waypoints.getWaypointPosition(i); marker.scale.x = 1.0; marker.scale.y = 1.0; marker.scale.z = 1.0; marker.color.a = 1.0; marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 1.0; marker.frame_locked = true; _vis_pub.publish(marker); }
bool SpecificWorker::gotoCommand(InnerModel *innerModel, CurrentTarget &target, TrajectoryState &state, WayPoints &myRoad, RoboCompLaser::TLaserData &lData) { QTime reloj = QTime::currentTime(); ///////////////////////////////////////////////////// // check for ending conditions ////////////////////////////////////////////////////// if (myRoad.isFinished() == true) { controller->stopTheRobot(omnirobot_proxy); qDebug() << __FUNCTION__ << "Changing to SETHEADING command"; target.setState(CurrentTarget::State::SETHEADING); return true; } if (myRoad.isBlocked() == true) //Road BLOCKED, go to BLOCKED state and wait it the obstacle moves { controller->stopTheRobot(omnirobot_proxy); //currentTargetBack.setTranslation(innerModel->transform("world", QVec::vec3(0, 0, -250), "robot")); target.setState(CurrentTarget::State::BLOCKED); return false; } // Get here when robot is stuck // if(myRoad.requiresReplanning == true) // { // //qDebug() << __FUNCTION__ << "STUCK, PLANNING REQUIRED"; // //computePlan(innerModel); // } ////////////////////////////////////////// // Check if there is a plan for the target ////////////////////////////////////////// bool coolPlan = true; if (target.isWithoutPlan() == true) { state.setState("PLANNING"); QVec localT = target.getTranslation(); coolPlan = plannerPRM.computePath(localT, innerModel); if (coolPlan == false) { qDebug() << __FUNCTION__ << "Path NOT found. Resetting to IDLE state"; target.setState(CurrentTarget::State::STOP); return false; } target.setTranslation(localT); //qDebug() << __FUNCTION__ << "Plan obtained of " << planner->getPath().size() << " points"; // take inner to current values updateInnerModel(innerModel, state); target.setWithoutPlan(false); //Init road REMOVE TRASH FROM HERE myRoad.reset(); myRoad.readRoadFromList(plannerPRM.getPath()); myRoad.requiresReplanning = false; myRoad.computeDistancesToNext(); myRoad.startRoad(); state.setPlanningTime(reloj.elapsed()); state.setState("EXECUTING"); } /////////////////////////////////// // Update the band ///////////////////////////////// elasticband.update(innerModel, myRoad, laserData, target); /////////////////////////////////// // compute all measures relating the robot to the road ///////////////////////////////// myRoad.update(); //myRoad.printRobotState(innerModel, target); ///////////////////////////////////////////////////// //move the robot according to the current force field ////////////////////////////////////////////////////// controller->update(innerModel, lData, omnirobot_proxy, myRoad); #ifdef USE_QTGUI waypointsdraw.draw(myRoad, viewer, target); #endif state.setEstimatedTime(myRoad.getETA()); return true; }
static void DisplayDetectionRange(const int &crosswalk_id, const int &num, const EControl &kind) { // set up for marker array visualization_msgs::MarkerArray marker_array; visualization_msgs::Marker crosswalk_marker; visualization_msgs::Marker waypoint_marker_stop; visualization_msgs::Marker waypoint_marker_decelerate; visualization_msgs::Marker stop_line; crosswalk_marker.header.frame_id = "/map"; crosswalk_marker.header.stamp = ros::Time(); crosswalk_marker.id = 0; crosswalk_marker.type = visualization_msgs::Marker::SPHERE_LIST; crosswalk_marker.action = visualization_msgs::Marker::ADD; waypoint_marker_stop = crosswalk_marker; waypoint_marker_decelerate = crosswalk_marker; stop_line = crosswalk_marker; stop_line.type = visualization_msgs::Marker::CUBE; // set each namespace crosswalk_marker.ns = "Crosswalk Detection"; waypoint_marker_stop.ns = "Stop Detection"; waypoint_marker_decelerate.ns = "Decelerate Detection"; stop_line.ns = "Stop Line"; // set scale and color double scale = 2*_detection_range; waypoint_marker_stop.scale.x = scale; waypoint_marker_stop.scale.y = scale; waypoint_marker_stop.scale.z = scale; waypoint_marker_stop.color.a = 0.2; waypoint_marker_stop.color.r = 0.0; waypoint_marker_stop.color.g = 1.0; waypoint_marker_stop.color.b = 0.0; waypoint_marker_stop.frame_locked = true; scale = 2*(_detection_range + _deceleration_range); waypoint_marker_decelerate.scale.x = scale; waypoint_marker_decelerate.scale.y = scale; waypoint_marker_decelerate.scale.z = scale; waypoint_marker_decelerate.color.a = 0.15; waypoint_marker_decelerate.color.r = 1.0; waypoint_marker_decelerate.color.g = 1.0; waypoint_marker_decelerate.color.b = 0.0; waypoint_marker_decelerate.frame_locked = true; if (_obstacle_waypoint > -1) { stop_line.pose.position = _path_dk.getWaypointPosition(_obstacle_waypoint); stop_line.pose.orientation = _path_dk.getWaypointOrientation(_obstacle_waypoint); } stop_line.pose.position.z += 1.0; stop_line.scale.x = 0.1; stop_line.scale.y = 15.0; stop_line.scale.z = 2.0; stop_line.color.a = 0.3; stop_line.color.r = 1.0; stop_line.color.g = 0.0; stop_line.color.b = 0.0; stop_line.lifetime = ros::Duration(0.1); stop_line.frame_locked = true; if (crosswalk_id > 0) scale = vmap.getDetectionPoints(crosswalk_id).width; crosswalk_marker.scale.x = scale; crosswalk_marker.scale.y = scale; crosswalk_marker.scale.z = scale; crosswalk_marker.color.a = 0.5; crosswalk_marker.color.r = 0.0; crosswalk_marker.color.g = 1.0; crosswalk_marker.color.b = 0.0; crosswalk_marker.frame_locked = true; // set marker points coordinate for (int i = 0; i < _search_distance; i++) { if (num < 0 || i+num > _path_dk.getSize() - 1) break; geometry_msgs::Point point; point = _path_dk.getWaypointPosition(num+i); waypoint_marker_stop.points.push_back(point); if (i > _deceleration_search_distance) continue; waypoint_marker_decelerate.points.push_back(point); } if (crosswalk_id > 0) { for (const auto &p : vmap.getDetectionPoints(crosswalk_id).points) crosswalk_marker.points.push_back(p); } // publish marker marker_array.markers.push_back(crosswalk_marker); marker_array.markers.push_back(waypoint_marker_stop); marker_array.markers.push_back(waypoint_marker_decelerate); if (kind == STOP) marker_array.markers.push_back(stop_line); _range_pub.publish(marker_array); marker_array.markers.clear(); }
static EControl vscanDetection() { if (_vscan.empty() == true || _closest_waypoint < 0) return KEEP; int decelerate_or_stop = -10000; int decelerate2stop_waypoints = 15; for (int i = _closest_waypoint; i < _closest_waypoint + _search_distance; i++) { g_obstacle.clearStopPoints(); if (!g_obstacle.isDecided()) g_obstacle.clearDeceleratePoints(); decelerate_or_stop++; if (decelerate_or_stop > decelerate2stop_waypoints || (decelerate_or_stop >= 0 && i >= _path_dk.getSize()-1) || (decelerate_or_stop >= 0 && i == _closest_waypoint+_search_distance-1)) return DECELERATE; if (i > _path_dk.getSize() - 1 ) return KEEP; // Detection for cross walk if (i == vmap.getDetectionWaypoint()) { if (CrossWalkDetection(vmap.getDetectionCrossWalkID()) == STOP) { _obstacle_waypoint = i; return STOP; } } // waypoint seen by vehicle geometry_msgs::Point waypoint = calcRelativeCoordinate(_path_dk.getWaypointPosition(i), _current_pose.pose); tf::Vector3 tf_waypoint = point2vector(waypoint); tf_waypoint.setZ(0); int stop_point_count = 0; int decelerate_point_count = 0; for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) { tf::Vector3 vscan_vector((double) item->x, (double) item->y, 0); // for simulation if (g_sim_mode) { tf::Transform transform; tf::poseMsgToTF(_sim_ndt_pose.pose, transform); geometry_msgs::Point world2vscan = vector2point(transform * vscan_vector); vscan_vector = point2vector(calcRelativeCoordinate(world2vscan, _current_pose.pose)); vscan_vector.setZ(0); } // 2D distance between waypoint and vscan points(obstacle) // ---STOP OBSTACLE DETECTION--- double dt = tf::tfDistance(vscan_vector, tf_waypoint); if (dt < _detection_range) { stop_point_count++; geometry_msgs::Point vscan_temp; vscan_temp.x = item->x; vscan_temp.y = item->y; vscan_temp.z = item->z; if (g_sim_mode) g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose)); else g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose)); } if (stop_point_count > _threshold_points) { _obstacle_waypoint = i; return STOP; } // without deceleration range if (_deceleration_range < 0.01) continue; // deceleration search runs "decelerate_search_distance" waypoints from closest if (i > _closest_waypoint+_deceleration_search_distance || decelerate_or_stop >= 0) continue; // ---DECELERATE OBSTACLE DETECTION--- if (dt > _detection_range && dt < _detection_range + _deceleration_range) { bool count_flag = true; // search overlaps between DETECTION range and DECELERATION range for (int waypoint_search = -5; waypoint_search <= 5; waypoint_search++) { if (i+waypoint_search < 0 || i+waypoint_search >= _path_dk.getSize() || !waypoint_search) continue; geometry_msgs::Point temp_waypoint = calcRelativeCoordinate( _path_dk.getWaypointPosition(i+waypoint_search), _current_pose.pose); tf::Vector3 waypoint_vector = point2vector(temp_waypoint); waypoint_vector.setZ(0); // if there is a overlap, give priority to DETECTION range if (tf::tfDistance(vscan_vector, waypoint_vector) < _detection_range) { count_flag = false; break; } } if (count_flag) { decelerate_point_count++; geometry_msgs::Point vscan_temp; vscan_temp.x = item->x; vscan_temp.y = item->y; vscan_temp.z = item->z; if (g_sim_mode) g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose)); else g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose)); } } // found obstacle to DECELERATE if (decelerate_point_count > _threshold_points) { _obstacle_waypoint = i; decelerate_or_stop = 0; // for searching near STOP obstacle g_obstacle.setDecided(true); } } } return KEEP; //no obstacles }
float ElasticBand::computeForces(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData) { if (road.size() < 3) return 0; // To avoid moving the rotation element attached to the last int lastP; if (road.last().hasRotation) lastP = road.size() - 2; else lastP = road.size() - 1; // Go through all points in the road float totalChange = 0.f; for (int i = 1; i < lastP; i++) { if (road[i].isVisible == false) break; WayPoint &w0 = road[i - 1]; WayPoint &w1 = road[i]; WayPoint &w2 = road[i + 1]; // Atraction force caused by the trajectory stiffnes, trying to straighten itself. It is computed as a measure of local curvature QVec atractionForce(3); float n = (w0.pos - w1.pos).norm2() / ((w0.pos - w1.pos).norm2() + w1.initialDistanceToNext); atractionForce = (w2.pos - w0.pos) * n - (w1.pos - w0.pos); //Compute derivative of force field and store values in w1.bMinuxX .... and w1.minDist. Also variations wrt former epochs computeDistanceField(innermodel, w1, laserData, FORCE_DISTANCE_LIMIT); QVec repulsionForce = QVec::zeros(3); QVec jacobian(3); // space interval to compute the derivative. Related to to robot's size float h = DELTA_H; if ((w1.minDistHasChanged == true) /*and (w1.minDist < 250)*/ ) { jacobian = QVec::vec3(w1.bMinusX - w1.bPlusX, 0, w1.bMinusY - w1.bPlusY) * (T) (1.f / (2.f * h)); // repulsion force is computed in the direction of maximun laser-point distance variation and scaled so it is 0 is beyond FORCE_DISTANCE_LIMIT and FORCE_DISTANCE_LIMIT if w1.minDist. repulsionForce = jacobian * (FORCE_DISTANCE_LIMIT - w1.minDist); } float alpha = -0.5; //Negative values between -0.1 and -1. The bigger in magnitude, the stiffer the road becomes float beta = 0.55; //Posibite values between 0.1 and 1 The bigger in magnitude, more separation from obstacles QVec change = (atractionForce * alpha) + (repulsionForce * beta); if (std::isnan(change.x()) or std::isnan(change.y()) or std::isnan(change.z())) { road.print(); qDebug() << atractionForce << repulsionForce; qFatal("change"); } //Now we remove the tangencial component of the force to avoid recirculation of band points //QVec pp = road.getTangentToCurrentPoint().getPerpendicularVector(); //QVec nChange = pp * (pp * change); w1.pos = w1.pos - change; totalChange = totalChange + change.norm2(); } return totalChange; }
bool LineFollower::update(InnerModel *innerModel, RoboCompLaser::TLaserData &laserData, RoboCompOmniRobot::OmniRobotPrx omnirobot_proxy, WayPoints &road) { static QTime reloj = QTime::currentTime(); //TO be used for a more accurate control (predictive). /*static*/ long epoch = 100; static float lastVadvance = 0.f; const float umbral = 25.f; //salto maximo de velocidad static float lastVrot = 0.f; const float umbralrot = 0.08f; //salto maximo de rotación //Estimate the space that will be blindly covered and reduce Adv speed to remain within some boundaries //qDebug() << __FILE__ << __FUNCTION__ << "entering update with" << road.at(road.getIndexOfClosestPointToRobot()).pos; //Check robot state if( (road.isFinished() == true ) or (road.requiresReplanning== true) or (road.isLost == true)) { if( road.isFinished() ) qDebug() << "road finished"; if( road.requiresReplanning ) qDebug() << "requiresReplanning"; if( road.isLost ) qDebug() << "robot is lost"; stopTheRobot(omnirobot_proxy); return false; } ///CHECK ROBOT INMINENT COLLISION float vside = 0; int j=0; road.setBlocked(false); for(auto i : laserData) { //printf("laser dist %f || baseOffsets %f \n",i.dist,baseOffsets[j]); if(i.dist < 10) i.dist = 30000; if( i.dist < baseOffsets[j] + 50 ) { if(i.angle>-1.30 && i.angle<1.30){ qDebug() << __FILE__ << __FUNCTION__<< "Robot stopped to avoid collision because distance to obstacle is less than " << baseOffsets[j] << " "<<i.dist << " " << i.angle; stopTheRobot(omnirobot_proxy); road.setBlocked(true); //AQUI SE BLOQUEA PARA REPLANIFICAR qDebug()<<"DETECTADO OBSTACULO, REPLANIFICANDO"; break; } } else { if (i.dist < baseOffsets[j] + 150) { if (i.angle > 0) { vside = -80; } else { vside = 80; } } } j++; } ///////////////////////////////////////////////// ////// CHECK CPU AVAILABILITY ///////////////////////////////////////////////// if ( time.elapsed() > delay ) //Initial wait in secs so the robot waits for everything is setup. Maybe it could be moved upwards { float MAX_ADV_SPEED = 200.f; float MAX_ROT_SPEED = 0.3; if( (epoch-100) > 0 ) //Damp max speeds if elapsed time is too long { MAX_ADV_SPEED = 200 * exponentialFunction(epoch-100, 200, 0.2); MAX_ROT_SPEED = 0.3 * exponentialFunction(epoch-100, 200, 0.2); } float vadvance = 0; float vrot = 0; ///////////////////////////////////////////////// ////// ROTATION SPEED //////////////////////////////////////////////// // VRot is computed as the sum of three terms: angle with tangent to road + atan(perp. distance to road) + road curvature // as descirbed in Thrun's paper on DARPA challenge vrot = road.getAngleWithTangentAtClosestPoint() + atan( road.getRobotPerpendicularDistanceToRoad()/800.) + 0.8 * road.getRoadCurvatureAtClosestPoint() ; //350->800. // Limiting filter if( vrot > MAX_ROT_SPEED ) vrot = MAX_ROT_SPEED; if( vrot < -MAX_ROT_SPEED ) vrot = -MAX_ROT_SPEED; ///////////////////////////////////////////////// ////// ADVANCE SPEED //////////////////////////////////////////////// // Factor to be used in speed control when approaching the end of the road float teta; if( road.getRobotDistanceToTarget() < 1000) teta = exponentialFunction(1./road.getRobotDistanceToTarget(),1./500,0.5, 0.1); else teta= 1; // Factor to be used in speed control when approaching the end of the road //VAdv is computed as a reduction of MAX_ADV_SPEED by three computed functions: // * road curvature reduces forward speed // * VRot reduces forward speed // * reduction is 1 if there are not obstacle. // * teta that applies when getting close to the target (1/roadGetCurvature) // * a Delta that takes 1 if approaching the target is true, 0 otherwise. It applies only if at less than 1000m to the target vadvance = MAX_ADV_SPEED * exp(-fabs(1.6 * road.getRoadCurvatureAtClosestPoint())) * exponentialFunction(vrot, 0.8, 0.01) * teta; //* exponentialFunction(1./road.getRobotDistanceToTarget(),1./500,0.5, 0.1) //* sunk; if(fabs(vrot - lastVrot) > umbralrot) { //qDebug()<<"lastrot "<<lastVrot << "\n vrot "<< vrot; if(vrot > lastVrot) vrot = lastVrot + umbralrot; else vrot = lastVrot - umbralrot; } lastVrot=vrot; //Pre-limiting filter to avoid displacements in very closed turns if( fabs(vrot) == 0.3) vadvance = 0; vside = 0; //stopping speed jump if(fabs(vadvance - lastVadvance) > umbral) { //qDebug()<<"lastadvanced "<<lastVadvance << "\n vadvance "<< vadvance; if(vadvance > lastVadvance) vadvance = lastVadvance + umbral; else vadvance = lastVadvance - umbral; } lastVadvance=vadvance; // Limiting filter if( vadvance > MAX_ADV_SPEED ) vadvance = MAX_ADV_SPEED; //vside = vrot*MAX_ADV_SPEED; ///////////////////////////////////////////////// ////// LOWEST-LEVEL COLLISION AVOIDANCE CONTROL //////////////////////////////////////////////// //bool collision = avoidanceControl(innerModel, laserData, vadvance, vrot); // if( collision ) // road.setBlocked(true); ///////////////////////////////////////////////// /// SIDEWAYS LASTMINUTE AVOIDING WITH THE OMNI BASE ///////////////////////////////////////////////// //TODO: PROBAR EN URSUS A VER COMO QUEDA.. // std::sort(laserData.begin(), laserData.end(), [](auto a, auto b){ return a.dist < b.dist;}); // if(laserData.front().dist > 300 && vside == 0)// and fabs(laserData.front().angle)>0.3) // { // if( laserData.front().angle > 0) vside = -30; // else vside = 30; // } ///////////////////////////////////////////////// ////// EXECUTION //////////////////////////////////////////////// // qDebug() << "------------------LineFollower Report ---------------;"; // qDebug() <<" VAdv: " << vadvance << "|\nVRot: " << vrot << "\nVSide: " << vside; // qDebug() << "---------------------------------------------------;"; try { omnirobot_proxy->setSpeedBase(vside, vadvance, vrot);} catch (const Ice::Exception &e) { std::cout << e << "Omni robot not responding" << std::endl; } } else //Too long delay. Stopping robot. { qDebug() << __FILE__ << __FUNCTION__ << "Processing delay" << epoch << "ms. too high. Stopping the robot for safety"; try { omnirobot_proxy->setSpeedBase( 0, 0, 0); } catch (const Ice::Exception &e) { std::cout << e << "Omni robot not responding" << std::endl; } } epoch = reloj.restart(); //epoch time in ms return false; }
//linear interpolation of next target static bool interpolateNextTarget(int next_waypoint,geometry_msgs::Point *next_target) { double search_radius = getLookAheadThreshold(0); geometry_msgs::Point zero_p; geometry_msgs::Point end = _current_waypoints.getWaypointPosition(next_waypoint); geometry_msgs::Point start = _current_waypoints.getWaypointPosition(next_waypoint - 1); //let the linear equation be "y = slope * x + intercept" double slope = 0; double intercept = 0; getLinearEquation(start,end,&slope, &intercept); //let the center of circle be "(x0,y0)", in my code , the center of circle is vehicle position //the distance "d" between the foot of a perpendicular line and the center of circle is ... // | y0 - slope * x0 - intercept | //d = ------------------------------- // √( 1 + slope^2) double d = getDistanceBetweenLineAndPoint(_current_pose.pose.position,slope,intercept); //ROS_INFO("slope : %lf ", slope); //ROS_INFO("intercept : %lf ", intercept); //ROS_INFO("distance : %lf ", d); if (d > search_radius) return false; //unit vector of point 'start' to point 'end' tf::Vector3 v((end.x - start.x), (end.y - start.y), 0); tf::Vector3 unit_v = v.normalize(); //normal unit vectors of v tf::Vector3 unit_w1 = rotateUnitVector(unit_v, 90); //rotate to counter clockwise 90 degree tf::Vector3 unit_w2 = rotateUnitVector(unit_v, -90); //rotate to counter clockwise 90 degree //the foot of a perpendicular line geometry_msgs::Point h1; h1.x = _current_pose.pose.position.x + d * unit_w1.getX(); h1.y = _current_pose.pose.position.y + d * unit_w1.getY(); h1.z = _current_pose.pose.position.z; geometry_msgs::Point h2; h2.x = _current_pose.pose.position.x + d * unit_w2.getX(); h2.y = _current_pose.pose.position.y + d * unit_w2.getY(); h2.z = _current_pose.pose.position.z; double error = pow(10, -5); //0.00001 //ROS_INFO("error : %lf", error); //ROS_INFO("whether h1 on line : %lf", h1.y - (slope * h1.x + intercept)); //ROS_INFO("whether h2 on line : %lf", h2.y - (slope * h2.x + intercept)); //check which of two foot of a perpendicular line is on the line equation geometry_msgs::Point h; if (fabs(h1.y - (slope * h1.x + intercept)) < error) { h = h1; // ROS_INFO("use h1"); } else if (fabs(h2.y - (slope * h2.x + intercept)) < error) { // ROS_INFO("use h2"); h = h2; } else { return false; } //get intersection[s] //if there is a intersection if (d == search_radius) { *next_target = h; return true; } else { //if there are two intersection //get intersection in front of vehicle double s = sqrt(pow(search_radius, 2) - pow(d, 2)); geometry_msgs::Point target1; target1.x = h.x + s * unit_v.getX(); target1.y = h.y + s * unit_v.getY(); target1.z = _current_pose.pose.position.z; geometry_msgs::Point target2; target2.x = h.x - s * unit_v.getX(); target2.y = h.y - s * unit_v.getY(); target2.z = _current_pose.pose.position.z; //ROS_INFO("target1 : ( %lf , %lf , %lf)", target1.x, target1.y, target1.z); //ROS_INFO("target2 : ( %lf , %lf , %lf)", target2.x, target2.y, target2.z); displayLinePoint(slope, intercept, target1, target2, h); //debug tool //check intersection is between end and start double interval = getPlaneDistance(end,start); if (getPlaneDistance(target1, end) < interval) { //ROS_INFO("result : target1"); *next_target = target1; return true; } else if (getPlaneDistance(target2, end) < interval) { //ROS_INFO("result : target2"); *next_target = target2; return true; } else { //ROS_INFO("result : false "); return false; } } }
static void WayPointCallback(const waypoint_follower::laneConstPtr &msg) { _current_waypoints.setPath(*msg); _waypoint_set = true; //ROS_INFO_STREAM("waypoint subscribed"); }