更新代码

mkr2018_mrc/robot_coordination/scripts/robot_control.py

@@ -15,11 +15,10 @@ import sys
 
 # Trajectory as a tuple of (x, y, time)
 g_trajectory = (
-    (1.8, 6.6, 0),
-    (2.8, 6.6, 4),
-    (2.8, 7.6, 8),
-    (1.8, 7.6, 12),
-    (1.8, 6.6, 16),
+    (5.3, 7.4, 10),
+    (2.8, 6.6, 10),
+    (2.8, 7.6, 10),
+    (1.8, 7.6, 12)
 )
 
 
@@ -66,16 +65,16 @@ def stop_movement(server_ns):
 def waypoint_list_from_tuple(traj):
     """Return a list of Waypoint instances from a list of (x, y, timepoint)"""
     waypoint_list = []
-    #for x, y, timepoint in traj:
-    for i in range(1, 5):
+    for x, y, timepoint in traj:
+    #for i in range(1, 5):
         waypoint = Waypoint()
         waypoint.pose = Pose()
-        waypoint.pose.position.x = random.uniform(0.8, 6.0)
-        waypoint.pose.position.y = random.uniform(0.8, 3.0)
-        waypoint.timepoint = rospy.Duration.from_sec(10)
-        #waypoint.pose.position.x = x
-        #waypoint.pose.position.y = y
-        #waypoint.timepoint = rospy.Duration.from_sec(timepoint)
+        #waypoint.pose.position.x = random.uniform(0.8, 6.0)
+        #waypoint.pose.position.y = random.uniform(0.8, 3.0)
+        #waypoint.timepoint = rospy.Duration.from_sec(10)
+        waypoint.pose.position.x = x
+        waypoint.pose.position.y = y
+        waypoint.timepoint = rospy.Duration.from_sec(timepoint)
         waypoint_list.append(waypoint)
     return waypoint_list
 

mkr2018_mrc/robot_coordination/src/robot.cpp

@@ -20,9 +20,9 @@ Robot::Robot() :
 
 void Robot::odometryCallback (const nav_msgs::Odometry::ConstPtr& msg) 
 {
-  ROS_INFO("got odometry");
+  //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);
+  ROS_INFO_STREAM("robot current pose_x:" << robot_pose_.position.x << " ,pose_y:" << robot_pose_.position.y);
 
   if (execute_) 
   {
@@ -50,9 +50,10 @@ bool Robot::addPathService(robot_coordination::AddPath::Request& req, robot_coor
   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++) {
+  for (int i =0; i < req.waypoints.size(); i++) 
+  {
     plan_.push_back(Waypoint(req.waypoints[i].pose, req.waypoints[i].timepoint));
-    ROS_DEBUG_STREAM("added waypoint (" <<
+    ROS_INFO_STREAM("added waypoint (" <<
         req.waypoints[i].pose.position.x <<
         ", " <<
         req.waypoints[i].pose.position.y <<
@@ -88,7 +89,7 @@ bool Robot::addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, int rrtStepSize
     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);
+    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));
@@ -98,17 +99,18 @@ bool Robot::addNewPointtoRRT(RRT &myRRT, RRT::rrtNode &tempNode, int rrtStepSize
         tempNode.parentID = nearestNodeID;
         tempNode.nodeID = myRRT.getTreeSize();
         myRRT.addNewNode(tempNode);
-
+        ROS_INFO("addNewPointtoRRT return true");
         return true;
     }
     else
+        ROS_INFO("addNewPointtoRRT return false");
         return false;
 }
 
 void Robot::generateTempPoint(RRT::rrtNode &tempNode)
 {
-    int x = rand() % 150 + 1;
-    int y = rand() % 150 + 1;
+    int x = rand() % 7 + 1;
+    int y = rand() % 11 + 1;
     tempNode.posX = x;
     tempNode.posY = y;
 }
@@ -121,18 +123,20 @@ void Robot::addBranchtoRRTTree(RRT::rrtNode &tempNode, RRT &myRRT)
   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)
+    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("distance:" << distance);
+    if(distance < 1.5)
     {
+        ROS_INFO("checkNodetoGoal return true");
         return true;
     }
+
+    ROS_INFO("checkNodetoGoal return false");
     return false;
 }
 
@@ -207,13 +211,13 @@ bool Robot::followTheCarrot()
     RRT::rrtNode tempNode;
 
     //Param input other robot current pose.x, pose.y
-    vector< vector<geometry_msgs::Point> >  obstacleList = getObstacles(0.1, 0.1);
+    vector< vector<geometry_msgs::Point> >  obstacleList = getObstacles(0.01, 0.01);
 
     bool addNodeResult = false, nodeToGoal = false;
     bool status = running;
+    srand(time(NULL));
     while(ros::ok() && status)
     {
-        srand(time(NULL));
         if(rrtPaths.size() < rrtPathLimit)
         {
             generateTempPoint(tempNode);
@@ -221,7 +225,7 @@ bool Robot::followTheCarrot()
             if(addNodeResult)
             {
                 ROS_INFO("tempnode accepted");
-                addBranchtoRRTTree(tempNode, myRRT);
+                //addBranchtoRRTTree(tempNode, myRRT);
                 nodeToGoal = checkNodetoGoal(goalX, goalY, tempNode);
                 if(nodeToGoal)
                 {