diff --git a/automow_bringup/launch/minimal.launch b/automow_bringup/launch/minimal.launch
index 0ecd0e0..78e3dc0 100755
--- a/automow_bringup/launch/minimal.launch
+++ b/automow_bringup/launch/minimal.launch
@@ -9,13 +9,15 @@
-
+
+
+
-
-
+
+
diff --git a/automow_description/urdf/automow.urdf.xacro b/automow_description/urdf/automow.urdf.xacro
index 2ef5835..b94883a 100755
--- a/automow_description/urdf/automow.urdf.xacro
+++ b/automow_description/urdf/automow.urdf.xacro
@@ -224,7 +224,7 @@
-
+
diff --git a/automow_maps/launch/field_publisher.launch b/automow_maps/launch/field_publisher.launch
old mode 100644
new mode 100755
index b8d109e..deee624
--- a/automow_maps/launch/field_publisher.launch
+++ b/automow_maps/launch/field_publisher.launch
@@ -1,6 +1,6 @@
+ default="$(find automow_maps)/maps/survey_solarhouse_may30.yaml" />
diff --git a/automow_maps/maps/survey_solarhouse_may30.yaml b/automow_maps/maps/survey_solarhouse_may30.yaml
new file mode 100644
index 0000000..ece74b0
--- /dev/null
+++ b/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'}
diff --git a/automow_planning/launch/path_planning.launch b/automow_planning/launch/path_planning.launch
old mode 100644
new mode 100755
index 2807fc7..87ca524
--- a/automow_planning/launch/path_planning.launch
+++ b/automow_planning/launch/path_planning.launch
@@ -6,5 +6,6 @@
output="screen">
+
\ No newline at end of file
diff --git a/automow_planning/manifest.xml b/automow_planning/manifest.xml
old mode 100644
new mode 100755
index 5670e8a..7d58336
--- a/automow_planning/manifest.xml
+++ b/automow_planning/manifest.xml
@@ -17,6 +17,8 @@
+
+
diff --git a/automow_planning/scripts/cutter_control.py b/automow_planning/scripts/cutter_control.py
old mode 100644
new mode 100755
index e63cc2c..6c59a1d
--- a/automow_planning/scripts/cutter_control.py
+++ b/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)
diff --git a/automow_planning/scripts/path_planner.py b/automow_planning/scripts/path_planner.py
index 6333272..a1f6788 100755
--- a/automow_planning/scripts/path_planner.py
+++ b/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,7 +44,7 @@ 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
@@ -51,7 +52,7 @@ def __init__(self):
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,8 +333,8 @@ 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]
@@ -333,20 +342,20 @@ def step_path_following(self):
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__':