增加了rrt算法，彼此避障待调试

mkr2018_mrc/path_planning/path_planning/include/path_planning/rrt.h

@@ -4,10 +4,9 @@
 #include <vector>
 using namespace std;
 namespace rrt {
-	class RRT{
-
+	class RRT
+    {
         public:
-
             RRT();
             RRT(double input_PosX, double input_PosY);
 

mkr2018_mrc/path_planning/path_planning/src/rrt_node.cpp

@@ -73,21 +73,20 @@ vector< vector<geometry_msgs::Point> > getObstacles()
 
 void addBranchtoRRTTree(visualization_msgs::Marker &rrtTreeMarker, RRT::rrtNode &tempNode, RRT &myRRT)
 {
+    geometry_msgs::Point point;
 
-geometry_msgs::Point point;
-
-point.x = tempNode.posX;
-point.y = tempNode.posY;
-point.z = 0;
-rrtTreeMarker.points.push_back(point);
+    point.x = tempNode.posX;
+    point.y = tempNode.posY;
+    point.z = 0;
+    rrtTreeMarker.points.push_back(point);
 
-RRT::rrtNode parentNode = myRRT.getParent(tempNode.nodeID);
+    RRT::rrtNode parentNode = myRRT.getParent(tempNode.nodeID);
 
-point.x = parentNode.posX;
-point.y = parentNode.posY;
-point.z = 0;
+    point.x = parentNode.posX;
+    point.y = parentNode.posY;
+    point.z = 0;
 
-rrtTreeMarker.points.push_back(point);
+    rrtTreeMarker.points.push_back(point);
 }
 
 bool checkIfInsideBoundary(RRT::rrtNode &tempNode)
@@ -183,7 +182,7 @@ int main(int argc,char** argv)
 	ros::NodeHandle n;
 
 	//defining Publisher
-	ros::Publisher rrt_publisher = n.advertise<visualization_msgs::Marker>("path_planner_rrt",1);
+	ros::Publisher rrt_publisher = n.advertise<visualization_msgs::Marker>("path_planner_rrt", 1);
 
 	//defining markers
     visualization_msgs::Marker sourcePoint;
@@ -210,7 +209,7 @@ int main(int argc,char** argv)
     //initialize rrt specific variables
 
     //initializing rrtTree
-    RRT myRRT(2.0,2.0);
+    RRT myRRT(2.0, 2.0);
     int goalX, goalY;
     goalX = goalY = 95;
 

mkr2018_mrc/robot_coordination/CMakeLists.txt

@@ -133,7 +133,7 @@ include_directories(
 # add_dependencies(robot ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
- add_executable(robot_node src/node.cpp src/robot.cpp src/waypoint.cpp)
+ add_executable(robot_node src/node.cpp src/robot.cpp src/waypoint.cpp src/rrt.cpp src/obstacles.cpp)
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above

mkr2018_mrc/robot_coordination/include/robot/obstacles.h

@@ -0,0 +1,22 @@
+#ifndef OBSTACLES_H
+#define OBSTACLES_H
+#include <geometry_msgs/Point.h>
+#include <vector>
+#include <iostream>
+
+using namespace std;
+
+class obstacles
+{
+    public:
+        /** Default constructor */
+        obstacles() {}
+        /** Default destructor */
+        virtual ~obstacles() {}
+        vector< vector<geometry_msgs::Point> > getObstacleArray(int pose_x, int pose_y);
+    protected:
+    private:
+        vector< vector<geometry_msgs::Point> > obstacleArray;
+};
+
+#endif // OBSTACLES_H

mkr2018_mrc/robot_coordination/include/robot/robot.h

@@ -1,13 +1,17 @@
-#include <list>
+#ifndef _ROBOT_H_
+#define _ROBOT_H_
 
+#include <list>
+#include <vector>
 #include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
-
+#include "robot/rrt.h"
 #include <robot/waypoint.h>
 #include <robot_coordination/AddPath.h>
 #include <robot_coordination/StartMovement.h>
 #include <robot_coordination/StopMovement.h>
 
+
 /** Control a robot with a follow-the-carrot strategy
  *
  * Plan is loaded directly with setPath() or through a service call of type AddPath.
@@ -16,9 +20,9 @@
  *
  * Not thread-safe.
  */
-class Robot {
+class Robot 
+{
   public:
-
     Robot();
 
     /** Callback function to process odometry messages.
@@ -42,6 +46,7 @@ class Robot {
      * As soon as the motion is finished, the state of the robot is changed to Waiting.
      */
     void startExecutePath();
+
     /** Stop the plan execution.
      * The robot stops at the spot and the state of the robot is changed to Waiting.
      */
@@ -53,6 +58,14 @@ class Robot {
     /** Create the publisher inside of the class*/
     void createCommandPublisher(ros::NodeHandle& nh);
 
+    bool addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, int rrtStepSize, vector< vector<geometry_msgs::Point> > &obstArray);
+    void generateTempPoint(RRT::rrtNode &tempNode);
+    bool checkNodetoGoal(int X, int Y, RRT::rrtNode &tempNode);
+    void addBranchtoRRTTree(RRT::rrtNode &tempNode, RRT &myRRT);
+    vector< vector<geometry_msgs::Point> > getObstacles(int pose_x, int pose_y);
+    bool checkIfOutsideObstacles(vector< vector<geometry_msgs::Point> > &obstArray, RRT::rrtNode &tempNode);
+    bool checkIfInsideBoundary(RRT::rrtNode &tempNode);
+
   private:
     /** Implementation of the follow-the-carrot control law.
      * This function is to be called in the odometry callback function.
@@ -73,3 +86,5 @@ class Robot {
 
     bool execute_;
 };
+
+#endif

mkr2018_mrc/robot_coordination/include/robot/rrt.h

@@ -0,0 +1,50 @@
+#ifndef rrt_h
+#define rrt_h
+
+#include <vector>
+using namespace std;
+
+class RRT
+{
+    public:
+        RRT();
+        RRT(double input_PosX, double input_PosY);
+
+        struct rrtNode
+        {
+            int nodeID;
+            double posX;
+            double posY;
+            int parentID;
+            vector<int> children;
+        };
+
+        vector<rrtNode> getTree();
+        void setTree(vector<rrtNode> input_rrtTree);
+        int getTreeSize();
+
+        void addNewNode(rrtNode node);
+        rrtNode removeNode(int nodeID);
+        rrtNode getNode(int nodeID);
+
+        double getPosX(int nodeID);
+        double getPosY(int nodeID);
+        void setPosX(int nodeID, double input_PosX);
+        void setPosY(int nodeID, double input_PosY);
+
+        rrtNode getParent(int nodeID);
+        void setParentID(int nodeID, int parentID);
+
+        void addChildID(int nodeID, int childID);
+        vector<int> getChildren(int nodeID);
+        int getChildrenSize(int nodeID);
+
+        int getNearestNodeID(double X, double Y);
+        vector<int> getRootToEndPath(int endNodeID);
+
+    private:
+        vector<rrtNode> rrtTree;
+        double getEuclideanDistance(double sourceX, double sourceY, double destinationX, double destinationY);
+};
+
+#endif

mkr2018_mrc/robot_coordination/include/robot/waypoint.h

@@ -1,12 +1,18 @@
+#ifndef _WAYPOINT_H_
+#define _WAYPOINT_H_
+
 #include "geometry_msgs/Pose.h"
 #include "ros/ros.h"
-class Waypoint {
+
+class Waypoint 
+{
   public:
-  geometry_msgs::Pose pose;
-  ros::Duration  timepoint;
-  Waypoint();
-  Waypoint (geometry_msgs::Pose, ros::Duration);
-  double distanceTo(geometry_msgs::Pose);
-  double angleTo(geometry_msgs::Pose);
-  
+    geometry_msgs::Pose pose;
+    ros::Duration  timepoint;
+    Waypoint();
+    Waypoint (geometry_msgs::Pose, ros::Duration);
+    double distanceTo(geometry_msgs::Pose);
+    double angleTo(geometry_msgs::Pose);
 };
+
+#endif

mkr2018_mrc/robot_coordination/scripts/robot_control.py

@@ -97,7 +97,6 @@ def add_path(server_ns, trajectory):
 
 def main():
     rospy.init_node('example_robot_control')
-
     server_ns_parameter_name = '~server_namespace'
     try:
         server_ns = rospy.get_param(server_ns_parameter_name)

mkr2018_mrc/robot_coordination/src/node.cpp

@@ -27,13 +27,16 @@ int main(int argc, char **argv)
   /*set the subscriber of the odometry to the robot*/
   ros::Subscriber odometry_subscriber = nh.subscribe("odom", 100, &Robot::odometryCallback, &robot);
   /*create a publiser of commands*/
-  ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist> ("cmd_vel",10);
+  ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist> ("cmd_vel", 10);
   /*advertise the service to add a path for a robot*/
   ros::ServiceServer service_add_path = nh.advertiseService("add_path", &Robot::addPathService, &robot);
+
   /*advertise the service to start the execution of the plan*/
   ros::ServiceServer service_start = nh.advertiseService("start_movement", &Robot::startMovementService, &robot);
+
   /*advertise the service to stop execution of the plan*/
   ros::ServiceServer service_stop = nh.advertiseService("stop_movement", &Robot::stopMovementService, &robot);
+
   /*set the publisher to the robot*/
   robot.setCommandPublisher(&cmd_pub);
 //  robot.startExecutePath();

mkr2018_mrc/robot_coordination/src/obstacles.cpp

@@ -0,0 +1,42 @@
+#include <robot/obstacles.h>
+#include <geometry_msgs/Point.h>
+
+//param pose_x,pose_y other robot current pose
+vector< vector<geometry_msgs::Point> > obstacles::getObstacleArray(int pose_x, int pose_y)
+{
+    vector<geometry_msgs::Point> obstaclePoint;
+    geometry_msgs::Point point;
+
+    //first point, left-top point
+    point.x = pose_x-0.5104;
+    point.y = pose_y+0.5104;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //second point, left-down point
+    point.x = pose_x-0.5104;
+    point.y = pose_y-0.5104;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //third point, right-down point
+    point.x = pose_x+0.5104;
+    point.y = pose_y-0.5104;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //fourth point, right-top point
+    point.x = pose_x+0.5104;
+    point.y = pose_y+0.5104;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //first point again to complete the box
+    point.x = pose_x-0.5104;
+    point.y = pose_y+0.5104;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    obstacleArray.push_back(obstaclePoint);
+    return obstacleArray;
+}

mkr2018_mrc/robot_coordination/src/robot.cpp

@@ -4,26 +4,40 @@
 #include "tf/tf.h"
 #include "robot/robot.h"
 #include "geometry_msgs/Twist.h"
+#include "robot/rrt.h"
+#include <vector>
+#include "robot/obstacles.h"
+#include "robot/waypoint.h"
+#include <time.h>
+#include "geometry_msgs/Pose.h"
 
+#define success false
+#define running true
 
 Robot::Robot() :
   threshold_(0.1) {
 }
 
-void Robot::odometryCallback (const nav_msgs::Odometry::ConstPtr& msg) {
-  ROS_DEBUG("got odometry");
-    robot_pose_ = msg->pose.pose;
-  if (execute_) {
+void Robot::odometryCallback (const nav_msgs::Odometry::ConstPtr& msg) 
+{
+  ROS_INFO("got odometry");
+  robot_pose_ = msg->pose.pose;
+  ROS_INFO_STREAM("robot pose_x:" << robot_pose_.position.x << "pose_y:" << robot_pose_.position.y);
+
+  if (execute_) 
+  {
     followTheCarrot();
   }
 }
 
-void Robot::startExecutePath() {
+void Robot::startExecutePath() 
+{
   execute_start_time_ = ros::Time::now();
   execute_ = true;
 }
 
-void Robot::stopExecutePath() {
+void Robot::stopExecutePath() 
+{
   geometry_msgs::Twist command;
   command.angular.z = 0;
   command.linear.x = 0;
@@ -31,12 +45,13 @@ void Robot::stopExecutePath() {
   execute_ = false;
 }
 
-bool Robot::addPathService(robot_coordination::AddPath::Request& req, robot_coordination::AddPath::Response& res) {
-  ROS_INFO("service addPath called");
+bool Robot::addPathService(robot_coordination::AddPath::Request& req, robot_coordination::AddPath::Response& res) 
+{
+  ROS_WARN("service addPath called");
   plan_.clear();
   ROS_INFO_STREAM("Plan has " << req.waypoints.size() << " points");
   for (int i =0; i < req.waypoints.size(); i++) {
-    plan_.push_back(Waypoint(req.waypoints[i].pose,req.waypoints[i].timepoint));
+    plan_.push_back(Waypoint(req.waypoints[i].pose, req.waypoints[i].timepoint));
     ROS_DEBUG_STREAM("added waypoint (" <<
         req.waypoints[i].pose.position.x <<
         ", " <<
@@ -68,8 +83,96 @@ void Robot::setCommandPublisher(ros::Publisher * cmdpub) {
   command_publisher_ = cmdpub;
 }
 
-bool Robot::followTheCarrot() {
-  if (plan_.empty()) {
+bool Robot::addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, int rrtStepSize, vector< vector<geometry_msgs::Point> > &obstArray)
+{
+    int nearestNodeID = myRRT.getNearestNodeID(tempNode.posX, tempNode.posY);
+    RRT::rrtNode nearestNode = myRRT.getNode(nearestNodeID);
+
+    double theta = atan2(tempNode.posY - nearestNode.posY,tempNode.posX - nearestNode.posX);
+
+    tempNode.posX = nearestNode.posX + (rrtStepSize * cos(theta));
+    tempNode.posY = nearestNode.posY + (rrtStepSize * sin(theta));
+
+    if(checkIfInsideBoundary(tempNode) && checkIfOutsideObstacles(obstArray,tempNode))
+    {
+        tempNode.parentID = nearestNodeID;
+        tempNode.nodeID = myRRT.getTreeSize();
+        myRRT.addNewNode(tempNode);
+
+        return true;
+    }
+    else
+        return false;
+}
+
+void Robot::generateTempPoint(RRT::rrtNode &tempNode)
+{
+    int x = rand() % 150 + 1;
+    int y = rand() % 150 + 1;
+    tempNode.posX = x;
+    tempNode.posY = y;
+}
+
+void Robot::addBranchtoRRTTree(RRT::rrtNode &tempNode, RRT &myRRT)
+{
+  geometry_msgs::Point point;
+  point.x = tempNode.posX;
+  point.y = tempNode.posY;
+  point.z = 0;
+
+  RRT::rrtNode parentNode = myRRT.getParent(tempNode.nodeID);
+  point.x = parentNode.posX;
+  point.y = parentNode.posY;
+  point.z = 0;
+}
+
+bool Robot::checkNodetoGoal(int X, int Y, RRT::rrtNode &tempNode)
+{
+    double distance = sqrt(pow(X-tempNode.posX,2)+pow(Y-tempNode.posY,2));
+    if(distance < 3)
+    {
+        return true;
+    }
+    return false;
+}
+
+vector< vector<geometry_msgs::Point> > Robot::getObstacles(int pose_x, int pose_y)
+{
+    obstacles obst;
+    return obst.getObstacleArray(pose_x, pose_y);
+}
+
+bool Robot::checkIfOutsideObstacles(vector< vector<geometry_msgs::Point> > &obstArray, RRT::rrtNode &tempNode)
+{
+    double AB, AD, AMAB, AMAD;
+
+    for(int i=0; i<obstArray.size(); i++)
+    {
+        AB = (pow(obstArray[i][0].x - obstArray[i][1].x,2) + pow(obstArray[i][0].y - obstArray[i][1].y,2));
+        AD = (pow(obstArray[i][0].x - obstArray[i][3].x,2) + pow(obstArray[i][0].y - obstArray[i][3].y,2));
+        AMAB = (((tempNode.posX - obstArray[i][0].x) * (obstArray[i][1].x - obstArray[i][0].x)) + (( tempNode.posY - obstArray[i][0].y) * (obstArray[i][1].y - obstArray[i][0].y)));
+        AMAD = (((tempNode.posX - obstArray[i][0].x) * (obstArray[i][3].x - obstArray[i][0].x)) + (( tempNode.posY - obstArray[i][0].y) * (obstArray[i][3].y - obstArray[i][0].y)));
+        
+        if((0 < AMAB) && (AMAB < AB) && (0 < AMAD) && (AMAD < AD))
+        {
+            return false;
+        }
+    }
+    return true;
+}
+
+bool Robot::checkIfInsideBoundary(RRT::rrtNode &tempNode)
+{
+    if(tempNode.posX < 0 || tempNode.posY < 0  || tempNode.posX > 100 || tempNode.posY > 100 ) 
+      return false;
+    else 
+      return true;
+}
+
+bool Robot::followTheCarrot() 
+{
+  if (plan_.empty()) 
+  {
     geometry_msgs::Twist command;
     command.angular.z = 0;
     command.linear.x = 0;
@@ -82,22 +185,93 @@ bool Robot::followTheCarrot() {
     {
       plan_.pop_front();
     }
-
+    
+    // rrt path planner
     Waypoint actual_goal = plan_.front();
-    const double dist = actual_goal.distanceTo(robot_pose_);
-    const double angle = actual_goal.angleTo(robot_pose_);
+    //initializing rrtTree
+    RRT myRRT(robot_pose_.position.x, robot_pose_.position.y);
+    int goalX = 0;
+    int goalY = 0;
+    goalX = actual_goal.pose.position.x;
+    goalY = actual_goal.pose.position.y;
+
+    int rrtStepSize = 3;
+
+    vector< vector<int> > rrtPaths;
+    vector<int> path;
+    int rrtPathLimit = 1;
+
+    int shortestPathLength = 9999;
+    int shortestPath = -1;
+
+    RRT::rrtNode tempNode;
+
+    //Param input other robot current pose.x, pose.y
+    vector< vector<geometry_msgs::Point> >  obstacleList = getObstacles(0.1, 0.1);
+
+    bool addNodeResult = false, nodeToGoal = false;
+    bool status = running;
+    while(ros::ok() && status)
+    {
+        srand(time(NULL));
+        if(rrtPaths.size() < rrtPathLimit)
+        {
+            generateTempPoint(tempNode);
+            addNodeResult = addNewPointtoRRT(myRRT, tempNode, rrtStepSize, obstacleList);
+            if(addNodeResult)
+            {
+                ROS_INFO("tempnode accepted");
+                addBranchtoRRTTree(tempNode, myRRT);
+                nodeToGoal = checkNodetoGoal(goalX, goalY, tempNode);
+                if(nodeToGoal)
+                {
+                    path = myRRT.getRootToEndPath(tempNode.nodeID);
+                    rrtPaths.push_back(path);
+                    ROS_WARN_STREAM("New Path Found. Total paths " << rrtPaths.size());
+                }
+            }
+        }
+        else //if(rrtPaths.size() >= rrtPathLimit)
+        {
+            status = success;
+            ROS_WARN("Finding Optimal Path !");
+            for(int i=0; i<rrtPaths.size(); i++)
+            {
+                if(rrtPaths[i].size() < shortestPath)
+                {
+                    shortestPath = i;
+                    shortestPathLength = rrtPaths[i].size();
+                }
+            }
+        }
+    }//end while
+
+    //default method move origin to goal.
+    #if 1
+    Waypoint first;
+    geometry_msgs::Pose temp_pose;
+    first.pose = temp_pose;
+    first.pose.position.x = myRRT.getNode(1).posX;
+    first.pose.position.y = myRRT.getNode(1).posY;
+    first.timepoint = ros::Duration(10.0);
+
+    Waypoint temp_goal = first;
+    const double dist  = temp_goal.distanceTo(robot_pose_);
+    const double angle = temp_goal.angleTo(robot_pose_);
     tf::Quaternion q(
         robot_pose_.orientation.x,
         robot_pose_.orientation.y,
         robot_pose_.orientation.z,
         robot_pose_.orientation.w);
     tf::Matrix3x3 m(q);
+
     double roll;
     double pitch;
     double yaw;
     m.getRPY(roll, pitch, yaw);
     ROS_INFO("distance %f and angle %f to goal", dist,angle);
     ROS_INFO("robot roll %f , pitch %f yaw %f", roll, pitch, yaw);
+
     geometry_msgs::Twist command;
     double delta = angle - yaw;
     if (delta > M_PI ) {
@@ -108,25 +282,27 @@ bool Robot::followTheCarrot() {
     }
     ROS_INFO_STREAM(ros::Time::now() << " " << actual_goal.timepoint);
     const double time_to_goal = ((execute_start_time_ + actual_goal.timepoint) - ros::Time::now()).toSec();
-
     ROS_INFO("time to goal %f ", time_to_goal);
+
     double speed;
-    if (time_to_goal > 0) {
+    if (time_to_goal > 0) 
+    {
       speed = dist/time_to_goal ;
-    } else {
-     speed = 1;
-     //plan_.clear();
+    }
+    else 
+    {
+      speed = 1;
     }
     command.angular.z = 2 * (sin(delta) - 0.333 * sin(3 * delta));
     command.angular.z = delta;
     command.linear.x = speed * (cos(delta) + 0.333 * cos(3 * delta));
-    if (command.linear.x < 0) {
+    if (command.linear.x < 0) 
+    {
       command.linear.x = 0;
     }
     command_publisher_->publish(command);
+    #endif
 
     return false;
   }
 }
-
-

mkr2018_mrc/robot_coordination/src/rrt.cpp

@@ -0,0 +1,212 @@
+#include <robot/rrt.h>
+#include <math.h>
+#include <cstddef>
+#include <iostream>
+
+
+/**
+* default constructor for RRT class
+* initializes source to 0,0
+* adds sorce to rrtTree
+*/
+RRT::RRT()
+{
+    RRT::rrtNode newNode;
+    newNode.posX = 0;
+    newNode.posY = 0;
+    newNode.parentID = 0;
+    newNode.nodeID = 0;
+    rrtTree.push_back(newNode);
+}
+
+/**
+* default constructor for RRT class
+* initializes source to input X,Y
+* adds sorce to rrtTree
+*/
+RRT::RRT(double input_PosX, double input_PosY)
+{
+    RRT::rrtNode newNode;
+    newNode.posX = input_PosX;
+    newNode.posY = input_PosY;
+    newNode.parentID = 0;
+    newNode.nodeID = 0;
+    rrtTree.push_back(newNode);
+}
+
+/**
+* Returns the current RRT tree
+* @return RRT Tree
+*/
+vector<RRT::rrtNode> RRT::getTree()
+{
+    return rrtTree;
+}
+
+/**
+* For setting the rrtTree to the inputTree
+* @param rrtTree
+*/
+void RRT::setTree(vector<RRT::rrtNode> input_rrtTree)
+{
+    rrtTree = input_rrtTree;
+}
+
+/**
+* to get the number of nodes in the rrt Tree
+* @return tree size
+*/
+int RRT::getTreeSize()
+{
+    return rrtTree.size();
+}
+
+/**
+* adding a new node to the rrt Tree
+*/
+void RRT::addNewNode(RRT::rrtNode node)
+{
+    rrtTree.push_back(node);
+}
+
+/**
+* removing a node from the RRT Tree
+* @return the removed tree
+*/
+RRT::rrtNode RRT::removeNode(int id)
+{
+    RRT::rrtNode tempNode = rrtTree[id];
+    rrtTree.erase(rrtTree.begin()+id);
+    return tempNode;
+}
+
+/**
+* getting a specific node
+* @param node id for the required node
+* @return node in the rrtNode structure
+*/
+RRT::rrtNode RRT::getNode(int id)
+{
+    return rrtTree[id];
+}
+
+/**
+* return a node from the rrt tree nearest to the given point
+* @param X position in X cordinate
+* @param Y position in Y cordinate
+* @return nodeID of the nearest Node
+*/
+int RRT::getNearestNodeID(double X, double Y)
+{
+    int i, returnID;
+    double distance = 9999, tempDistance;
+    for(i=0; i<this->getTreeSize(); i++)
+    {
+        tempDistance = getEuclideanDistance(X,Y, getPosX(i),getPosY(i));
+        if (tempDistance < distance)
+        {
+            distance = tempDistance;
+            returnID = i;
+        }
+    }
+
+    return returnID;
+}
+
+/**
+* returns X coordinate of the given node
+*/
+double RRT::getPosX(int nodeID)
+{
+    return rrtTree[nodeID].posX;
+}
+
+/**
+* returns Y coordinate of the given node
+*/
+double RRT::getPosY(int nodeID)
+{
+    return rrtTree[nodeID].posY;
+}
+
+/**
+* set X coordinate of the given node
+*/
+void RRT::setPosX(int nodeID, double input_PosX)
+{
+    rrtTree[nodeID].posX = input_PosX;
+}
+
+/**
+* set Y coordinate of the given node
+*/
+void RRT::setPosY(int nodeID, double input_PosY)
+{
+    rrtTree[nodeID].posY = input_PosY;
+}
+
+/**
+* returns parentID of the given node
+*/
+RRT::rrtNode RRT::getParent(int id)
+{
+    return rrtTree[rrtTree[id].parentID];
+}
+
+/**
+* set parentID of the given node
+*/
+void RRT::setParentID(int nodeID, int parentID)
+{
+    rrtTree[nodeID].parentID = parentID;
+}
+
+/**
+* add a new childID to the children list of the given node
+*/
+void RRT::addChildID(int nodeID, int childID)
+{
+    rrtTree[nodeID].children.push_back(childID);
+}
+
+/**
+* returns the children list of the given node
+*/
+vector<int> RRT::getChildren(int id)
+{
+    return rrtTree[id].children;
+}
+
+/**
+* returns number of children of a given node
+*/
+int RRT::getChildrenSize(int nodeID)
+{
+    return rrtTree[nodeID].children.size();
+}
+
+/**
+* returns euclidean distance between two set of X,Y coordinates
+*/
+double RRT::getEuclideanDistance(double sourceX, double sourceY, double destinationX, double destinationY)
+{
+    return sqrt(pow(destinationX - sourceX,2) + pow(destinationY - sourceY,2));
+}
+
+/**
+* returns path from root to end node
+* @param endNodeID of the end node
+* @return path containing ID of member nodes in the vector form
+*/
+vector<int> RRT::getRootToEndPath(int endNodeID)
+{
+    vector<int> path;
+    path.push_back(endNodeID);
+    while(rrtTree[path.front()].nodeID != 0)
+    {
+        //std::cout<<rrtTree[path.front()].nodeID<<endl;
+        path.insert(path.begin(), rrtTree[path.front()].parentID);
+    }
+    
+    return path;
+}

mkr2018_mrc/robot_coordination/src/waypoint.cpp

@@ -3,13 +3,15 @@
 Waypoint::Waypoint() {
 };
 
-Waypoint::Waypoint(geometry_msgs::Pose p, ros::Duration d) {
+Waypoint::Waypoint(geometry_msgs::Pose p, ros::Duration d) 
+{
   pose = p;
   timepoint = d;
 }
 
 /** return distance to given pose*distance to given pose*/
-double Waypoint::distanceTo(geometry_msgs::Pose p) {
+double Waypoint::distanceTo(geometry_msgs::Pose p) 
+{
   double d_x = pose.position.x - p.position.x;
   double d_y = pose.position.y - p.position.y;
   double dist = sqrt(d_x*d_x + d_y*d_y);
@@ -17,10 +19,10 @@ double Waypoint::distanceTo(geometry_msgs::Pose p) {
 }
 
 /** return relative angle to the given pose*/
-double Waypoint::angleTo(geometry_msgs::Pose p) {
+double Waypoint::angleTo(geometry_msgs::Pose p) 
+{
   double d_x = pose.position.x - p.position.x;
   double d_y = pose.position.y - p.position.y;
   double angle = atan2(d_y, d_x);
   return angle;
-
 }