Feature/nav stack setup

cr17/CMakeLists.txt

@@ -54,12 +54,14 @@ catkin_python_setup()
    localizationPoints.msg
    wheelData.msg
    scoopControl.msg
+   dockerStatus.msg
  )
 
 ## Generate services in the 'srv' folder
  add_service_files(
    FILES
    autonomousActive.srv
+   dockerState.srv
  )
 
 ## Generate actions in the 'action' folder

cr17/launch/move_base.launch

@@ -0,0 +1,19 @@
+<launch>
+  <!-- Run the map server -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find cr17)/param/rmc_field_map.yaml"/>
+
+  <!--- Run AMCL, which I think is not needed -->
+  <!--<include file="$(find amcl)/examples/amcl_diff.launch" />-->
+
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <rosparam file="$(find cr17)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find cr17)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find cr17)/param/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find cr17)/param/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find cr17)/param/base_local_planner_params.yaml" command="load" />
+    <remap from="/move_base_simple/goal" to="/nav_goal" />
+    <remap from="cmd_vel" to="/cr17_diff_drive_controller/cmd_vel"/>
+    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+    <param name="controller_frequency" value="10.0" />
+  </node>
+</launch>

cr17/msg/dockerStatus.msg

@@ -0,0 +1,2 @@
+bool dockerActive
+bool dockingComplete

cr17/nodes/cr17/docker.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python2
+import rospy
+from cr17.msg import dockerStatus
+from cr17.srv import dockerState, dockerStateResponse
+from geometry_msgs.msg import PoseStamped, Twist, Pose
+from tf.transformations import euler_from_quaternion, quaternion_from_euler
+
+
+POSE_TOPIC = rospy.get_param("topics/localization_pose")
+DOCKER_TOPIC = rospy.get_param("topics/docker_status")
+DRIVE_TOPIC = rospy.get_param("topics/drive_cmds")
+
+Y_TARGET = rospy.get_param("docker/y_target")
+Y_RANGE = rospy.get_param("docker/y_range")
+Y_SCALAR = rospy.get_param("docker/y_scalar")
+YAW_TARGET = rospy.get_param("docker/yaw_target")
+YAW_RANGE = rospy.get_param("docker/yaw_range")
+YAW_SCALAR = rospy.get_param("docker/yaw_scalar")
+
+
+class Docker(object):
+    def __init__(self):
+        rospy.init_node('docker')
+
+        self.dockerActive = False
+        self.pose = Pose()
+
+        self.dockingReverse = False
+
+        #ROS Publishers
+        self.drivePub = rospy.Publisher(DRIVE_TOPIC, Twist, queue_size = 10)
+        self.dockerStatusPub = rospy.Publisher(DOCKER_TOPIC, dockerStatus, queue_size = 10)
+
+        #ROS Subscribers
+        rospy.Subscriber(POSE_TOPIC, PoseStamped, self.pose_sub)
+
+        # ROS Services
+        rospy.Service('/dockerState', dockerState, self.set_docker_state)
+
+    def pose_sub(self, data):
+        self.pose = data.pose
+
+    def set_docker_state(self, data):
+        if (self.dockerActive != data.newDockerState) and (data.newDockerState):
+            self.dockingReverse = True
+        self.dockerActive = data.newDockerState
+        return dockerStateResponse(self.dockerActive)
+
+    def run(self):
+        rate = rospy.Rate(10)
+
+        while not rospy.is_shutdown():
+            currentStatus = dockerStatus()
+            currentStatus.dockerActive = self.dockerActive
+
+            if self.dockerActive:
+                currentY = self.pose.position.y
+                currentOrientation = euler_from_quaternion([self.pose.orientation.x, self.pose.orientation.y, self.pose.orientation.z, self.pose.orientation.w]) 
+                currentYaw = currentOrientation[2]
+
+                driveTwist = Twist()
+                driveTwist.linear.x = (currentY - Y_TARGET) * Y_SCALAR
+                driveTwist.angular.z = (currentYaw - YAW_TARGET) * YAW_SCALAR
+
+                currentStatus.dockingComplete = False
+
+                if abs(currentY - Y_TARGET) < Y_RANGE:
+                    if abs(currentYaw - YAW_TARGET) < YAW_RANGE:
+                        currentStatus.dockingComplete = True
+                        driveTwist.linear.x = 0
+                        driveTwist.angular.z = 0
+
+                self.drivePub.publish(driveTwist)
+
+            elif self.dockingReverse:
+                driveTwist = Twist()
+                driveTwist.linear.x = -20
+                for each in range(0, 30):
+                    self.drivePub.publish(driveTwist)
+                    rate.sleep()
+                self.dockingReverse = False
+
+            else:
+                currentStatus.dockingComplete = False
+
+            self.dockerStatusPub.publish(currentStatus)
+            rate.sleep()
+
+
+
+if __name__ == "__main__":
+	docker = Docker()
+	docker.run()

cr17/nodes/cr17/high_level_state_controller.py

@@ -5,20 +5,21 @@
 from geometry_msgs.msg import PoseStamped, Twist, Pose
 from std_msgs.msg import Float64, Bool, String
 from nav_msgs.msg import Path
-from cr17.msg import scoopControl
-from cr17.srv import autonomousActive, autonomousActiveResponse
+from cr17.msg import scoopControl, dockerStatus
+from cr17.srv import autonomousActive, autonomousActiveResponse, dockerState
 
 
 ARM_STATE_TOPIC = rospy.get_param("topics/scoop_state_cmds", "scoop_commands")
 POSE_TOPIC = rospy.get_param("topics/localization_pose") #rospy.get_param("topics/filtered_pose", "filtered_pose")
 PATH_TOPIC = rospy.get_param("topics/path", "/obstacle_path")
 GOAL_TOPIC = rospy.get_param("topics/navigation_goals", "nav_goal") #TODO: might need to be renamed
 STATE_TOPIC = rospy.get_param("topics/robot_state", "state")
+DOCKER_TOPIC = rospy.get_param("topics/docker_status")
 MINING_DISTANCE = rospy.get_param("distance_to_mine", 1.5)
 
-Y_IN_DUMP_RANGE = 0.6
+Y_IN_DUMP_RANGE = 0.4
 Y_IN_MINING_AREA = 4.5 #10 cm past edge of mining area
-Y_IN_DOCKING_AREA = 1.5
+Y_IN_DOCKING_AREA = 1.7
 
 TIME_DUMP_SEC = 10
 
@@ -40,25 +41,42 @@ def __init__(self):
         self.state_pub = rospy.Publisher(STATE_TOPIC, String, queue_size=10)
 
         self.pose = Pose()
-        self.pathPoses = []     #TODO: maybe remove, send entire current path to navigator
-        self.curPathIndex = 0   #TODO: maybe remove, send entire current path to navigator
         self.autostate = "INIT"
 
         self.miningAngleIndex = 0
-        self.miningPathIndex = 0
         self.miningReady = False
         self.miningDone = False
-        self.minePath = None
+        self.miningTarget = None
         self.dumpTimer = None
         self.autonomyEnabled = True
 
+        self.frontMiningGoal = PoseStamped()
+        self.frontMiningGoal.header.frame_id = "map"
+        self.frontMiningGoal.pose.position.x = 0
+        self.frontMiningGoal.pose.position.y = Y_IN_MINING_AREA + 0.4
+        self.frontMiningGoal.pose.orientation.x = 0
+        self.frontMiningGoal.pose.orientation.y = 0
+        self.frontMiningGoal.pose.orientation.z = -0.7071067811865476
+        self.frontMiningGoal.pose.orientation.w = -0.7071067811865476
+
+        self.dumpSetupGoal = PoseStamped()
+        self.dumpSetupGoal.header.frame_id = "map"
+        self.dumpSetupGoal.pose.position.x = 0
+        self.dumpSetupGoal.pose.position.y = Y_IN_DOCKING_AREA - 0.2
+        self.dumpSetupGoal.pose.orientation.x = 0
+        self.dumpSetupGoal.pose.orientation.y = 0
+        self.dumpSetupGoal.pose.orientation.z = -0.7071067811865476
+        self.dumpSetupGoal.pose.orientation.w = 0.7071067811865476
+
+        self.dockerStatus = dockerStatus()
+
         # ROS Publishers
         self.arm_pub = rospy.Publisher(ARM_STATE_TOPIC, scoopControl, queue_size=10)
         self.goal_pub = rospy.Publisher(GOAL_TOPIC, PoseStamped, queue_size=10) #TODO: change to a Path
 
         #ROS Subscribers
         rospy.Subscriber(POSE_TOPIC, PoseStamped, self.pose_sub)
-        rospy.Subscriber(PATH_TOPIC, Path, self.path_sub)
+        rospy.Subscriber(DOCKER_TOPIC, dockerStatus, self.docker_sub)
 
         # ROS Services
         self.auto_srv = rospy.Service('autonomousHLSC', autonomousActive, self.set_autonomy)
@@ -70,8 +88,8 @@ def set_autonomy(self, data):
     def pose_sub(self, data):
         self.pose = data.pose
 
-    def path_sub(self, data):
-        self.pathPoses = data.poses
+    def docker_sub(self, data):
+        self.dockerStatus = data
 
     def arm_drive_state(self):
         newMsg = scoopControl()
@@ -108,10 +126,7 @@ def arm_postdump_state(self):
         newMsg.desiredState = scoopControl.state_postDump
         self.arm_pub.publish(newMsg)
 
-    def calculate_mining_path(self, angle):
-        startPose = PoseStamped()
-        startPose.pose.position.x = self.pose.position.x
-        startPose.pose.position.y =Y_IN_MINING_AREA - 0.4#TODO: make this better, or improve the state transitions, this is so that the mining state will be left
+    def calculate_mining_target(self, angle):
         start_x = self.pose.position.x
         start_y = self.pose.position.y
 
@@ -123,14 +138,17 @@ def calculate_mining_path(self, angle):
         if abs(end_x) > 1.2:
             end_x = abs(end_x)/end_x * 1.2  #so it won't run into the wall
 
-        path = []
+        #this needs to include the orientation if we use the nav_stack, and since the orientation is a quaternion it will need conversion
         endPose = PoseStamped()
+        endPose.header.frame_id = "map"
         endPose.pose.position.x = end_x
         endPose.pose.position.y = end_y
+        endPose.pose.orientation.x = 0
+        endPose.pose.orientation.y = 0
+        endPose.pose.orientation.z = -0.7071067811865476
+        endPose.pose.orientation.w = -0.7071067811865476
 
-        path.append(startPose)
-        path.append(endPose)
-        return path
+        return endPose
 
     def close_to(self, poseChecked):
         try:
@@ -152,17 +170,12 @@ def run(self):
             if(self.autostate == "INIT"):
                 pass
                 ######################################################################################################
-                #TODO: add code to orient the robot properly
+                #TODO: add code to orient the robot properly, may just set the arm, not sure yet
                 ######################################################################################################
 
 
             elif(self.autostate == "F_OBSTACLE_FIELD"):
-                if(len(self.pathPoses) != 0):
-                    if(self.close_to(self.pathPoses[self.curPathIndex])):
-                        self.curPathIndex += 1
-                        if(self.curPathIndex >= len(self.pathPoses)):
-                            self.curPathIndex = len(self.pathPoses) - 1
-                    self.goal_pub.publish(self.pathPoses[self.curPathIndex])
+                self.goal_pub.publish(self.frontMiningGoal)
                 self.arm_drive_state()
 
             elif(self.autostate == "MINING_BEHAVIOR"):
@@ -176,31 +189,22 @@ def run(self):
                 elif(self.miningReady and not self.miningDone ):
                     self.arm_dig_state()
 
-                    if(self.minePath == None):
-                        self.minePath = self.calculate_mining_path(ANGLES_TO_MINE[self.miningAngleIndex])
+                    if(self.miningTarget == None):
+                        self.miningTarget = self.calculate_mining_target(ANGLES_TO_MINE[self.miningAngleIndex])
                         self.miningAngleIndex += 1
                         if(self.miningAngleIndex >= len(ANGLES_TO_MINE)):
                             self.miningAngleIndex = 0
-                        self.miningPathIndex = min(1, len(self.minePath) - 1) #skip the first point when going forward
 
-                    if(self.close_to(self.minePath[self.miningPathIndex])):
-                        self.miningPathIndex += 1
-                        if (self.miningPathIndex >= len(self.minePath)):
-                            self.miningPathIndex = len(self.minePath) - 1
-                    self.goal_pub.publish(self.minePath[self.miningPathIndex])
+                    self.goal_pub.publish(self.miningTarget)
 
-                    if(self.close_to(self.minePath[len(self.minePath) - 1])):
+                    if(self.close_to(self.miningTarget)):
                         self.miningDone = True
                         self.arm_postdig_state()
 
                 #once the end of the path has been reached, close the hand and iterate back across path
                 elif self.miningDone:
                     self.arm_postdig_state()
-                    if (self.close_to(self.minePath[self.miningPathIndex])):
-                        self.miningPathIndex -= 1
-                        if (self.miningPathIndex < 0):
-                            self.miningPathIndex = 0
-                    self.goal_pub.publish(self.minePath[self.miningPathIndex])
+                    self.goal_pub.publish(self.dumpSetupGoal)
 
                 #once the beginning has been reached again
 
@@ -209,30 +213,35 @@ def run(self):
             ######################################################################################################
 
             elif(self.autostate == "B_OBSTACLE_FIELD"):
+                self.goal_pub.publish(self.dumpSetupGoal)
                 self.arm_drive_state()
-                if(self.close_to(self.pathPoses[self.curPathIndex])):
-                    self.curPathIndex -= 1
-                    if(self.curPathIndex < 0 ):
-                        self.curPathIndex = 0
-                self.goal_pub.publish(self.pathPoses[self.curPathIndex])
 
             elif(self.autostate == "DOCKING"):
                 self.arm_predump_state()
-                dockPose = PoseStamped()
-                dockPose.pose.position.x = X_POS_DUMP
-                dockPose.pose.position.y = Y_POS_DUMP
-                self.goal_pub.publish(dockPose)
+                try:
+                    dockerService = rospy.ServiceProxy("/dockerState", dockerState)
+                    dockerService(True)
+                except rospy.ServiceException, e:
+                    rospy.logerr("/dockerState service call failed: %s",e)
 
             elif(self.autostate == "DUMPING"):
                 dumping_complete = False
                 if(self.dumpTimer == None):
-                    self.dumpTimer = rospy.Time.now() + rospy.Duration(TIME_DUMP_SEC)
+                    self.dumpTimer = rospy.Time.now() + rospy.Duration(secs = TIME_DUMP_SEC)
                 elif(self.dumpTimer >= rospy.Time.now() ):
                     dumping_complete = True
                     self.arm_postdump_state()
                 else:
                     self.arm_dump_state()
 
+            elif(self.autostate == "POST_DUMP"):
+                if self.dockerStatus.dockerActive:
+                    try:
+                        dockerService = rospy.ServiceProxy("/dockerState", dockerState)
+                        dockerService(False)
+                    except rospy.ServiceException, e:
+                        rospy.logerr("/dockerState service call failed: %s",e)
+
             ######################################################################################################
             #TODO: add code to turn the robot around
             ######################################################################################################
@@ -245,23 +254,23 @@ def run(self):
             elif(self.autostate == "F_OBSTACLE_FIELD"):
                 if(self.pose.position.y >= Y_IN_MINING_AREA):
                     self.autostate = "MINING_BEHAVIOR"
-                    self.curPathIndex = len(self.pathPoses) - 1
             elif(self.autostate == "MINING_BEHAVIOR"):
                 if( not self.autonomyEnabled or (self.pose.position.y < Y_IN_MINING_AREA - .3) ):
                     self.autostate = "B_OBSTACLE_FIELD"
-                    self.minePath = None
+                    self.miningTarget = None
                     self.miningDone = False
             elif(self.autostate == "B_OBSTACLE_FIELD"):
                 if(self.pose.position.y <= Y_IN_DOCKING_AREA):
                     self.autostate = "DOCKING"
-                    self.curPathIndex = 0
             elif(self.autostate == "DOCKING"):
-                if(self.pose.position.y < Y_IN_DUMP_RANGE):
+                if (self.pose.position.y < Y_IN_DUMP_RANGE) or self.dockerStatus.dockingComplete:
                     self.autostate = "DUMPING"
             elif(self.autostate == "DUMPING"):
                 if(dumping_complete or (self.pose.position.y >= Y_IN_DUMP_RANGE)):
-                    self.autostate = "F_OBSTACLE_FIELD"
+                    self.autostate = "POST_DUMP"
                     self.dumpTimer = None
+            elif(self.autostate == "POST_DUMP"):
+                self.autostate = "F_OBSTACLE_FIELD"
             else:
                 self.autostate = "INIT"