update code

mkr2018_mrc/robot_coordination/include/robot/robot.h

@@ -60,8 +60,7 @@ class Robot
 
     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);
+    bool checkNodetoGoal(float X, float Y, RRT::rrtNode &tempNode);
     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);

mkr2018_mrc/robot_coordination/launch/startup_robots_control.launch

@@ -11,12 +11,12 @@ History:
     <arg name="namespace_1" default="robot1" />
 
     <group ns="$(arg namespace_0)">
-        <node pkg="robot_coordination" type="robot_node" name="control_path" output="screen" required="true">
+        <node pkg="robot_coordination" type="robot_node" name="robot0_control_path" output="screen" required="true">
         </node>
     </group>
     
     <group ns="$(arg namespace_1)">
-        <node pkg="robot_coordination" type="robot_node" name="control_path" output="screen" required="true">
+        <node pkg="robot_coordination" type="robot_node" name="robot1_control_path" output="screen" required="true">
         </node>
     </group>
 </launch>

mkr2018_mrc/robot_coordination/scripts/robot_control.py

@@ -15,10 +15,10 @@ import sys
 
 # Trajectory as a tuple of (x, y, time)
 g_trajectory = (
-    (5.3, 7.4, 10),
-    (2.8, 6.6, 10),
-    (2.8, 7.6, 10),
-    (1.8, 7.6, 12)
+    (5.6, 1.9, 10),
+    (4.8, 3.6, 10),
+    (3.8, 2.6, 10),
+    (1.8, 1.6, 12)
 )
 
 

mkr2018_mrc/robot_coordination/src/robot.cpp

@@ -14,8 +14,8 @@
 #define success false
 #define running true
 
-Robot::Robot() :
-  threshold_(0.1) {
+Robot::Robot() :  threshold_(0.1) 
+{
 }
 
 void Robot::odometryCallback (const nav_msgs::Odometry::ConstPtr& msg) 
@@ -111,24 +111,15 @@ void Robot::generateTempPoint(RRT::rrtNode &tempNode)
 {
     int x = rand() % 7 + 1;
     int y = rand() % 11 + 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);
-}
-
-bool Robot::checkNodetoGoal(int X, int Y, RRT::rrtNode &tempNode)
+bool Robot::checkNodetoGoal(float X, float Y, RRT::rrtNode &tempNode)
 {
     double distance = sqrt(pow(X-tempNode.posX, 2)+pow(Y-tempNode.posY, 2));
-    ROS_INFO_STREAM("checkNodetoGoal: input x: " << X << " ,y:"<<Y<<" , tempNode.posX:"<<tempNode.posX<<", y:"<<tempNode.posY);
+    ROS_INFO_STREAM("checkNodetoGoal: input x: " << X << " ,y:"<<Y<<",tempNode.posX:"<<tempNode.posX<<",y:"<<tempNode.posY);
     ROS_INFO_STREAM("distance:" << distance);
     if(distance < 1.5)
     {
@@ -181,6 +172,7 @@ bool Robot::followTheCarrot()
     command.angular.z = 0;
     command.linear.x = 0;
     command_publisher_->publish(command);
+    
     return true;
   } 
   else 
@@ -192,6 +184,7 @@ bool Robot::followTheCarrot()
     
     // rrt path planner
     Waypoint actual_goal = plan_.front();
+
     //initializing rrtTree
     RRT myRRT(robot_pose_.position.x, robot_pose_.position.y);
     int goalX = 0;
@@ -199,7 +192,7 @@ bool Robot::followTheCarrot()
     goalX = actual_goal.pose.position.x;
     goalY = actual_goal.pose.position.y;
 
-    int rrtStepSize = 3;
+    int rrtStepSize = 1;
 
     vector< vector<int> > rrtPaths;
     vector<int> path;
@@ -224,8 +217,7 @@ bool Robot::followTheCarrot()
             addNodeResult = addNewPointtoRRT(myRRT, tempNode, rrtStepSize, obstacleList);
             if(addNodeResult)
             {
-                ROS_INFO("tempnode accepted");
-                //addBranchtoRRTTree(tempNode, myRRT);
+                //ROS_INFO("tempnode accepted");
                 nodeToGoal = checkNodetoGoal(goalX, goalY, tempNode);
                 if(nodeToGoal)
                 {
@@ -258,6 +250,8 @@ bool Robot::followTheCarrot()
     first.pose.position.x = myRRT.getNode(1).posX;
     first.pose.position.y = myRRT.getNode(1).posY;
     first.timepoint = ros::Duration(10.0);
+    ROS_WARN_STREAM("PATH first node x:"<<first.pose.position.x<<",y:"<<first.pose.position.y);
+    ROS_WARN_STREAM("CURRENT robot pose x:"<<robot_pose_.position.x<<",y"<<robot_pose_.position.y);
 
     Waypoint temp_goal = first;
     const double dist  = temp_goal.distanceTo(robot_pose_);
@@ -273,8 +267,8 @@ bool Robot::followTheCarrot()
     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);
+    //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;
@@ -284,7 +278,7 @@ bool Robot::followTheCarrot()
     if (delta < -M_PI ) {
       delta += 2 * M_PI;
     }
-    ROS_INFO_STREAM(ros::Time::now() << " " << actual_goal.timepoint);
+    //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);