Merge branch 'master' of ssh://mowbot/var/git/au_automow_common

automow_bringup/launch/minimal.launch

@@ -9,13 +9,15 @@
       <rosparam command="load" file="$(find automow_bringup)/config/diagnostics.yaml" />
   </node>
 
-  <node pkg="ax2550" type="ax2550_node" name="ax2550_node" output="screen" />
+  <node pkg="ax2550" type="ax2550_node" name="ax2550_node" output="screen">
+      <remap from="odom" to="ax2550/odom"/>
+  </node>
 
   <node name="hokuyo" pkg="hokuyo_node" type="hokuyo_node">
       <param name="port" type="str" value="/dev/laser" />
       <param name="frame_id" type="str" value="/laser" />
-      <param name="min_ang" value="-2.15" />
-      <param name="max_ang" value="2.2" />
+      <param name="min_ang" value="-2" />
+      <param name="max_ang" value="2" />
       <param name="cluster" value="1" />
       <param name="skip" value="0"/>
       <param name="intensity" value="false"/>

automow_description/urdf/automow.urdf.xacro

@@ -224,7 +224,7 @@
     </link>
 
     <joint name="laser_joint" type="fixed">
-        <origin xyz="0.9 0.17 ${0.145 - clearance}" rpy="${M_PI} 0 0.785398163"/>
+        <origin xyz="0.9 0.17 ${0.3302 - clearance}" rpy="0 0 0.785398163"/>
         <parent link="base_link" />
         <child link="laser" />
     </joint>

automow_maps/launch/field_publisher.launch

@@ -1,6 +1,6 @@
 <launch>
 	<arg name="field_file"
-		 default="$(find automow_maps)/maps/field_example.yaml" />
+		 default="$(find automow_maps)/maps/survey_solarhouse_may30.yaml" />
 
     <node pkg="automow_maps" type="field_publisher.py" name="field_publisher"
           output="screen" >

automow_maps/maps/survey_solarhouse_may30.yaml

@@ -0,0 +1,6 @@
+- {easting: '5.39350000001', fix_type: '1', northing: '20.8106000004'}
+- {easting: '5.98679999996', fix_type: '1', northing: '30.4512'}
+- {easting: '-4.56170000008', fix_type: '1', northing: '31.0984'}
+- {easting: '-4.80839999998', fix_type: '1', northing: '28.7768000001'}
+- {easting: '-2.69430000009', fix_type: '1', northing: '28.6569000003'}
+- {easting: '-2.98090000008', fix_type: '1', northing: '21.8544000001'}

automow_planning/launch/path_planning.launch

@@ -6,5 +6,6 @@
 		  output="screen">
 		<!-- Spacing between cut lines in meters -->
 		<param name="cut_spacing" type="double" value="0.5" />
+		<remap from="odom" to="ekf/odom"/>
 	</node>
 </launch>

automow_planning/manifest.xml

@@ -17,6 +17,8 @@
   <depend package="std_msgs"/>
   <depend package="automow_pcb"/>
   <depend package="automow_maps"/>
+  <depend package="actionlib"/>
+  <depend package="move_base_msgs"/>
 
 </package>
 

automow_planning/scripts/cutter_control.py

@@ -43,6 +43,7 @@ def __init__(self):
 
         # Setup publishers and subscribers
         rospy.Subscriber('/field_shape', PolygonStamped, self.field_callback)
+        self.listener = tf.TransformListener()
 
         # Setup ROS service
         set_cutter_states = rospy.ServiceProxy('cutters', Cutters)
@@ -58,6 +59,7 @@ def __init__(self):
         while not rospy.is_shutdown():
             exceptions = (tf.LookupException,
                           tf.ConnectivityException,
+                          tf.ExtrapolationException,
                           rospy.ServiceException)
             try:
                 # Update the proper cutter states
@@ -119,10 +121,10 @@ def check_cutters(self):
         if self.field_shape == None or self.field_frame_id == None:
             return
         # Get the left cutter
-        left_cutter = get_cutter_shape(self.field_frame_id,
+        left_cutter = self.get_cutter_shape(self.field_frame_id,
                                        self.left_cutter_frame_id)
         # Get the right cutter
-        right_cutter = get_cutter_shape(self.field_frame_id,
+        right_cutter = self.get_cutter_shape(self.field_frame_id,
                                         self.right_cutter_frame_id)
         # Check to see if the left cutter is in the field polygon
         left_cutter_state = self.is_cutter_in_field(left_cutter)

automow_planning/scripts/path_planner.py

@@ -17,6 +17,7 @@
 from actionlib import SimpleActionClient
 
 import numpy as np
+from math import radians
 
 from geometry_msgs.msg import PolygonStamped, Point, PoseStamped
 from std_msgs.msg import ColorRGBA
@@ -43,15 +44,15 @@ def __init__(self):
         self.path_marker_pub = rospy.Publisher('visualization_marker',
                                                MarkerArray,
                                                latch=True)
-        self.Subscriber('/odom', Odometry, self.odom_callback)
+        rospy.Subscriber('/odom', Odometry, self.odom_callback)
 
         # Setup initial variables
         self.field_shape = None
         self.field_frame_id = None
         self.path = None
         self.path_status = None
         self.path_markers = None
-        self.start_path_following = False
+        self.start_path_following = True
         self.robot_pose = None
         self.goal_state = None
         self.current_destination = None
@@ -111,7 +112,7 @@ def plan_path(self, field_polygon, origin=None):
         from automow_planning.maptools import rotate_from
         self.path = rotate_from(np.array(transformed_path), rotation)
         # Calculate headings and extend the waypoints with them
-        self.calculate_headings(self.path)
+        self.path = self.calculate_headings(self.path)
         # Set the path_status to 'not_visited'
         self.path_status = []
         for waypoint in self.path:
@@ -123,17 +124,21 @@ def calculate_headings(self, path):
         """
         Calculates the headings between paths and adds them to the waypoints.
         """
+        new_path = []
         for index, waypoint in enumerate(path):
+            new_path.append(list(path[index]))
             # If the end, copy the previous heading
             if index == len(path)-1:
-                path[index].append(path[index-1][2])
+                new_path[index].append(new_path[index-1][2])
                 continue
             # Calculate the angle between this waypoint and the next
             dx = path[index+1][0] - path[index][0]
             dy = path[index+1][1] - path[index][1]
-            from math import atan2
-            heading = atan2(dx, dy)
-            path[index].append(heading)
+            from math import atan2, pi
+            heading = atan2(dy, dx)
+            heading += pi
+            new_path[index].append(heading)
+        return new_path
 
     def visualize_path(self, path, path_status=None):
         """
@@ -256,9 +261,12 @@ def setup_path_following(self):
         connected_to_move_base = False
         dur = rospy.Duration(1.0)
         # Wait for the robot position
-        while self.robot_pose == None and not connected_to_move_base:
+        while self.robot_pose == None or not connected_to_move_base:
             # Wait for the server for a while
-            connected_to_move_base = self.move_base_client.wait_for_server(dur)
+            if not connected_to_move_base:
+                connected_to_move_base = self.move_base_client.wait_for_server(dur)
+            else:
+                rospy.Rate(1.0).sleep()
             # Check to make sure ROS is ok still
             if rospy.is_shutdown(): return
             # Update the user on the status of this process
@@ -272,6 +280,7 @@ def setup_path_following(self):
             msg += "before starting planning."
             rospy.loginfo(msg)
         # Now we should plan a path using the robot's initial pose
+        print(type(self.robot_pose))
         origin = (self.robot_pose.pose.pose.position.x,
                   self.robot_pose.pose.pose.position.y)
         self.plan_path(self.field_shape, origin)
@@ -324,29 +333,29 @@ def step_path_following(self):
             destination.target_pose.header.frame_id = self.field_frame_id
             destination.target_pose.header.stamp = rospy.Time.now()
             # Set the target location
-            destination.target_pose.pose.pose.position.x = current_waypoint[0]
-            destination.target_pose.pose.pose.position.y = current_waypoint[1]
+            destination.target_pose.pose.position.x = current_waypoint[0]
+            destination.target_pose.pose.position.y = current_waypoint[1]
             # Set the heading
             quat = qfe(0, 0, radians(current_waypoint[2]))
             destination.target_pose.pose.orientation.x = quat[0]
             destination.target_pose.pose.orientation.y = quat[1]
             destination.target_pose.pose.orientation.z = quat[2]
             destination.target_pose.pose.orientation.w = quat[3]
             # Send the desired destination to the actionlib server
-            rospy.loginfo("Sending waypoint (%f, %f)@%f"%current_waypoint)
+            rospy.loginfo("Sending waypoint (%f, %f)@%f" % tuple(current_waypoint))
             self.current_destination = destination
             self.move_base_client.send_goal(destination)
         # If the status is visiting, then we just need to monitor the status
         if current_waypoint_status == 'visiting':
-            temp_state = self.move_base_client.get_state()
+            temp_state = self.move_base_client.get_goal_status_text()
             # Figure out the msg and action based on the state
-            msg = "Current waypoint (%f, %f)@%f is " % current_waypoint
+            msg = "Current waypoint (%f, %f)@%f is " % tuple(current_waypoint)
             msg += temp_state
             if temp_state in ['ABORTED', 'SUCCEEDED']:
                 self.path_status[current_waypoint_index] = 'visited'
             else:
                 duration = rospy.Duration(1.0/10.0)
-                if self.move_base_client.wait_for_result(dur):
+                if self.move_base_client.wait_for_result(duration):
                     self.path_status[current_waypoint_index] = 'visited'
 
 if __name__ == '__main__':