update code

mkr2018_mrc/robot_coordination/include/robot/robot.h

@@ -58,12 +58,13 @@ 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);
+    bool addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, double rrtStepSize, vector< vector<geometry_msgs::Point> > &obstArray);
     void generateTempPoint(RRT::rrtNode &tempNode);
-    bool checkNodetoGoal(float X, float Y, RRT::rrtNode &tempNode);
+    bool checkNodetoGoal(double X, double 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);
+    float GetRand(  float fMin, float fMax  );
 
   private:
     /** Implementation of the follow-the-carrot control law.

mkr2018_mrc/robot_coordination/launch/startup_robots_control.launch

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

mkr2018_mrc/robot_coordination/src/obstacles.cpp

@@ -36,7 +36,103 @@ vector< vector<geometry_msgs::Point> > obstacles::getObstacleArray(double pose_x
     point.y = pose_y+0.5104;
     point.z = 0;
     obstaclePoint.push_back(point);
+    obstacleArray.push_back(obstaclePoint);
+
+    //------------------------------------//
+    //first point, left-top point
+    point.x = 4.66;
+    point.y = 3.673;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //second point, left-down point
+    point.x = 4.725;
+    point.y = 5.218;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //third point, right-down point
+    point.x = 0.215;
+    point.y = 5.226;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //fourth point, right-top point
+    point.x = 0.24;
+    point.y = 3.71;
+    point.z = 0;
+    obstaclePoint.push_back(point);
 
+    //first point again to complete the box
+    point.x = 4.66;
+    point.y = 3.673;
+    point.z = 0;
+    obstaclePoint.push_back(point);
     obstacleArray.push_back(obstaclePoint);
+
+    //-------------------------------------------//
+    //first point, left-top point
+    point.x = 5.63;
+    point.y = 4.177;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //second point, left-down point
+    point.x = 5.78;
+    point.y = 3.66;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //third point, right-down point
+    point.x = 6.415;
+    point.y = 3.641;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //fourth point, right-top point
+    point.x = 6.371;
+    point.y = 4.20;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //first point again to complete the box
+    point.x = 5.63;
+    point.y = 4.177;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+    obstacleArray.push_back(obstaclePoint);
+
+    //--------------------------------------------------//
+    //first point, left-top point
+    point.x = 0.308;
+    point.y = 11;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //second point, left-down point
+    point.x = 0.251;
+    point.y = 9.00;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //third point, right-down point
+    point.x = 3.87;
+    point.y = 9.02;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //fourth point, right-top point
+    point.x = 3.88;
+    point.y = 11;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+
+    //first point again to complete the box
+    point.x = 0.308;
+    point.y = 11;
+    point.z = 0;
+    obstaclePoint.push_back(point);
+    obstacleArray.push_back(obstaclePoint);
+
     return obstacleArray;
 }

mkr2018_mrc/robot_coordination/src/robot.cpp

@@ -1,5 +1,4 @@
 #include <cmath>  // For M_PI.
-
 #include "ros/ros.h"
 #include "tf/tf.h"
 #include "robot/robot.h"
@@ -15,7 +14,7 @@
 #define running true
 
 
-Robot::Robot() : threshold_(0.1) 
+Robot::Robot() : threshold_(1.3) 
 {
 
 }
@@ -86,7 +85,7 @@ void Robot::setCommandPublisher(ros::Publisher * cmdpub) {
   command_publisher_ = cmdpub;
 }
 
-bool Robot::addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, int rrtStepSize, vector< vector<geometry_msgs::Point> > &obstArray)
+bool Robot::addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, double rrtStepSize, vector< vector<geometry_msgs::Point> > &obstArray)
 {
     int nearestNodeID = myRRT.getNearestNodeID(tempNode.posX, tempNode.posY);
     RRT::rrtNode nearestNode = myRRT.getNode(nearestNodeID);
@@ -108,22 +107,48 @@ bool Robot::addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, int rrtStepSize
         ROS_INFO("addNewPointtoRRT return false");
         return false;
 }
+float Robot::GetRand(  float fMin, float fMax  )
+{
+	assert( fMax > fMin );
+	float fRand=0.0f, f1=0.0f;
+
+	int n;
+	float f;
+	n = static_cast<float>(fMax-fMin);
+	f = (fMax-fMin) - n;
+
+	if ( n>0 )
+		fRand = static_cast<float>( rand() % n );
+	else
+		fRand = 0.f;
+
+	do 
+	{
+		f1 = static_cast<float>( rand() ) / RAND_MAX;
+		f1 *= ( rand()%2==0 ? 1 : -1 );
+	} while ( fRand+f1<0 || fRand+f1 > (fMax-fMin) );
+
+	fRand += (fMin+f1);
+
+	return fRand;
+}
+
 
 void Robot::generateTempPoint(RRT::rrtNode &tempNode)
 {
-    int x = rand() % 7 + 1;
-    int y = rand() % 11 + 1;
-
-    tempNode.posX = x;
-    tempNode.posY = y;
+    tempNode.posX = GetRand(0.3, 6.5);
+    tempNode.posY = GetRand(0.3, 11.0);
+    ROS_WARN_STREAM("Name:"<< ros::this_node::getName() << ",tempNode.posX: " << tempNode.posX
+                   <<",tempNode.posY:" << tempNode.posY);
 }
 
-bool Robot::checkNodetoGoal(float X, float Y, RRT::rrtNode &tempNode)
+bool Robot::checkNodetoGoal(double X, double 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)
     {
         ROS_INFO("checkNodetoGoal return true");
@@ -187,6 +212,8 @@ bool Robot::followTheCarrot()
     
     // rrt path planner
     Waypoint actual_goal = plan_.front();
+    //ROS_WARN_STREAM("");
+
 
     //initializing rrtTree
     RRT myRRT(robot_pose_.position.x, robot_pose_.position.y);
@@ -195,14 +222,14 @@ bool Robot::followTheCarrot()
     goalX = actual_goal.pose.position.x;
     goalY = actual_goal.pose.position.y;
 
-    ROS_INFO_STREAM("robot current pose_x:" << double(robot_pose_.position.x) << " ,pose_y:" << double(robot_pose_.position.y)
-                    << " ,goal_x:" << goalX << " ,goal_y:" << goalY);
+    ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<" ,current pose_x:" << double(robot_pose_.position.x) << " ,pose_y:" 
+                    << double(robot_pose_.position.y) << " ,goal_x:" << goalX << " ,goal_y:" << goalY);
 
-    int rrtStepSize = 1;
+    double rrtStepSize = 0.5;
 
     vector< vector<int> > rrtPaths;
     vector<int> path;
-    int rrtPathLimit = 1;
+    int rrtPathLimit = 5;
 
     int shortestPathLength = 9999;
     int shortestPath = -1;
@@ -229,7 +256,7 @@ bool Robot::followTheCarrot()
                 {
                     path = myRRT.getRootToEndPath(tempNode.nodeID);
                     rrtPaths.push_back(path);
-                    ROS_WARN_STREAM("New Path Found. Total paths " << rrtPaths.size());
+                    ROS_WARN_STREAM("New Path Found. Total paths:" << rrtPaths.size()<<" ,NodeID:"<<tempNode.nodeID);
                 }
             }
         }
@@ -253,58 +280,67 @@ bool Robot::followTheCarrot()
     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.pose.position.x = myRRT.getNode(tempNode.nodeID).posX;
+    first.pose.position.y = myRRT.getNode(tempNode.nodeID).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);
+    ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<"PATH first node x:"<<first.pose.position.x<<",y:"<<first.pose.position.y);
+    ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<"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_);
-    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 ) {
-      delta -= 2 * M_PI;
-    }
-    if (delta < -M_PI ) {
-      delta += 2 * M_PI;
-    }
-    //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 dist = 1;
+    double angle = 0;
 
-    double speed;
-    if (time_to_goal > 0) 
+    //while(dist > 0.1)
     {
-      speed = dist/time_to_goal ;
+      dist  = temp_goal.distanceTo(robot_pose_);
+      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 ) {
+        delta -= 2 * M_PI;
+      }
+      if (delta < -M_PI ) {
+        delta += 2 * M_PI;
+      }
+
+      //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_STREAM("Name:"<< ros::this_node::getName() <<"time to goal:"<<time_to_goal<<" ,dist:"<<dist);
+      ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<"CURRENT robot pose x:"<<robot_pose_.position.x<<",y:"<<robot_pose_.position.y);
+      ROS_WARN_STREAM("Name:"<< ros::this_node::getName() <<"Goal pose x:" << first.pose.position.x << ",y:" << first.pose.position.y);
+
+      double speed;
+      if (time_to_goal > 0) 
+      {
+        speed = dist/time_to_goal;
+      }
+      else 
+      {
+        speed = 0.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) 
+      {
+        command.linear.x = 0;
+      }
+      command_publisher_->publish(command);
     }
-    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) 
-    {
-      command.linear.x = 0;
-    }
-    command_publisher_->publish(command);
     #endif
 
     return false;