diff --git a/nodes/coverage_progress b/nodes/coverage_progress index 51169ef..c95d88e 100755 --- a/nodes/coverage_progress +++ b/nodes/coverage_progress @@ -1,7 +1,7 @@ #! /usr/bin/env python import rospy -import tf +import tf2_ros from nav_msgs.msg import OccupancyGrid from numpy import ones, sum from std_msgs.msg import Float32, Header @@ -27,10 +27,9 @@ class CoverageProgressNode(object): DIRTY = 100 def __init__(self): - self.listener = tf.TransformListener() + self.tf_buffer = tf2_ros.Buffer() + self.listener = tf2_ros.TransformListener(self.tf_buffer) - x = None # type: float - y = None # type: float self._coverage_area = None # type: Tuple[float, float] self.coverage_resolution = None # type: float # How big is a cell [m] @@ -49,7 +48,7 @@ class CoverageProgressNode(object): self.reset_srv = rospy.Service('reset', Trigger, self.reset) self._rate = rospy.get_param("~rate", 10.0) - self._update_timer = rospy.Timer(rospy.Duration(1.0/self._rate), self._update_callback) + self._update_timer = rospy.Timer(rospy.Duration(1.0/self._rate), self._update_callback, reset=True) def _initialize_map(self): # Initialize coverage matrix @@ -98,17 +97,19 @@ class CoverageProgressNode(object): # Get the position of point (0,0,0) the coverage_disk frame wrt. the map frame (which can both be remapped if need be) try: - (coveragepos, rot) = self.listener.lookupTransform(self.map_frame, self.coverage_frame, rospy.Time(0)) - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): + coverage_tf = self.tf_buffer.lookup_transform(self.map_frame, self.coverage_frame, rospy.Time(0)) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.logwarn(e) return # Element of matrix corresponding to middle of coverage surface - x_point = int((coveragepos[X] - self.grid.info.origin.position.x) / self.coverage_resolution) - y_point = int((coveragepos[Y] - self.grid.info.origin.position.y) / self.coverage_resolution) + x_point = int((coverage_tf.transform.translation.x - self.grid.info.origin.position.x) / self.coverage_resolution) + y_point = int((coverage_tf.transform.translation.y - self.grid.info.origin.position.y) / self.coverage_resolution) # Initialize message self.grid.header = Header() self.grid.header.frame_id = self.map_frame + self.grid.header.stamp = coverage_tf.header.stamp # Loop over amount of cells covered in x (j) and y (i) direction for i in range(2 * self.coverage_radius_cells): @@ -126,9 +127,6 @@ class CoverageProgressNode(object): if cell_in_coverage_circle and cell_in_grid: self.grid.data[array_index] = max(0, self.grid.data[array_index] - self.coverage_effectivity) - else: - rospy.logdebug("x_point %i y_point %i, x_meas %f, y_meas %f", x_point, y_point, coveragepos[X], - coveragepos[Y]) coverage_progress = float(sum([self.grid.data < self.DIRTY])) / (self.grid.info.width * self.grid.info.height) diff --git a/package.xml b/package.xml index f03585e..37b2414 100644 --- a/package.xml +++ b/package.xml @@ -22,7 +22,7 @@ pluginlib nav_core roscpp - tf + tf2_ros amcl joint_state_publisher map_server diff --git a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch index ffa9c89..204557b 100644 --- a/test/full_coverage_path_planner/test_full_coverage_path_planner.launch +++ b/test/full_coverage_path_planner/test_full_coverage_path_planner.launch @@ -74,7 +74,7 @@ - +