def pathCallback(self, path_msg): wp_route_msg = WaypointRoute() # WaypointRoute msg wp_route_msg.speed = self.speed count = 0 first_wp_msg = self.getStationMsg( # Get first station path_msg.poses[0].pose, path_msg.poses[10].pose, path_msg.poses[0].pose, 2.50, True) wp_route_msg.waypoints.append(first_wp_msg) self.pubMarker(first_wp_msg.pose) for pose_stamped in path_msg.poses: count += 1 # Put at start to avoid setting the immediate waypoint if count % self.thinning_value == 0: wp_msg = Waypoint() # Waypoint msg wp_msg.nav_type = Waypoint.NAV_WAYPOINT wp_msg.pose = pose_stamped.pose self.pubMarker(wp_msg.pose) wp_route_msg.waypoints.append(wp_msg) # Append wp to route half_thin_value = round(self.thinning_value / 2) # Remove current last waypoint if too close to actual last waypoint if (len(path_msg.poses) % self.thinning_value) < half_thin_value: wp_route_msg.waypoints.pop() last_wp_msg = self.getStationMsg( # Get last station path_msg.poses[-11].pose, path_msg.poses[-1].pose, path_msg.poses[-1].pose, 0.0, False) wp_route_msg.waypoints.append(last_wp_msg) self.pubMarker(last_wp_msg.pose) self.wp_cmd_pub.publish(wp_route_msg)
def cb(self, data): #print("RECEIVED WAYPOINT MESSAGE") route = WaypointRoute() waypoints = [] for dps in data.poses: wp = Waypoint() wp.pose = dps.pose # it starts as a posestamped so extract the pose wp.nav_type = wp.NAV_STATION wp.station_duration = 3 # Wait 3 seconds at station to minimise error waypoints.append(wp) route.waypoints = waypoints route.speed = self.speed self.pub.publish(route)
def cb(self, data): print("RECEIVED WAYPOINT MESSAGE") route = WaypointRoute() waypoints = [] wp = Waypoint() wp.pose = data.pose wp.nav_type = wp.NAV_STATION wp.station_duration = -1 # Stay at station until received new command waypoints.append(wp) route.waypoints = waypoints route.speed = self.speed self.pub.publish(route)
def navigateToDirect(self, target, wait=True, timeout=0, dist_thresh=1, ang_thresh=0.4): """ Navigate to the location, if wait is True: Wait until destination is reached, if not, Not using the mission planner""" self.publishMarker(target) rospy.loginfo("Navigating to a location x: %f. y:%f", target.position.x, target.position.y) waypoint_msg = WaypointRoute() waypoint_msg.speed = 2 ##For Now, waypoints are 2 waypoints. At nav waypoint then a nav station loc2 = Waypoint() loc2.pose = target loc2.nav_type = Waypoint.NAV_STATION loc2.station_duration = -1 waypoint_msg.waypoints = [loc2] self.waypoint_pub.publish(waypoint_msg) start = rospy.Time.now().secs rate = rospy.Rate(20) expired = False while (not self.inRange( target, dist_thresh=dist_thresh, ang_thresh=ang_thresh)) and wait and not expired: #print(target,self.current_pose) rate.sleep() if timeout != 0 and (rospy.Time.now().secs - start) > timeout: rospy.loginfo("Timeing out: %f", rospy.Time.now().secs - start) expired = True #Republish a waypoint in its own position if wanted to wait, else, it will exit the function but continue on its trajectory. if wait: waypoint_msg = WaypointRoute() waypoint_msg.speed = 2 loc0 = Waypoint() loc0.pose = self.current_pose loc0.nav_type = Waypoint.NAV_STATION loc0.station_duration = -1 waypoint_msg.waypoints = [loc0] self.waypoint_pub.publish(waypoint_msg) rospy.loginfo("Arrived at target") return
def navigateTo(self, target, wait=True, timeout=0, dist_thresh=1, ang_thresh=0.4, repubish=False): self.publishMarker(target) # Navigate to the location, if wait is True: Wait until destination is reached, if not, self.navigateToDirect(target, wait=False, timeout=0) rospy.loginfo("Navigating to a location x: %f. y:%f", target.position.x, target.position.y) waypoint_msg = WaypointRoute() waypoint_msg.speed = 3 ##For Now, waypoints are 2 waypoints. At nav waypoint then a nav station goal = PoseStamped() goal.pose = target goal.header.frame_id = "map" goal.header.stamp = rospy.Time.now() self.goal_pub.publish(goal) start = rospy.Time.now().secs rate = rospy.Rate(20) expired = False while (not self.inRange( target, dist_thresh=dist_thresh, ang_thresh=ang_thresh)) and wait and not expired: rate.sleep() if timeout != 0 and rospy.Time.now().secs - start > timeout: expired = True #Republish a waypoint in its own position if wanted to wait, else, it will exit the function but continue on its trajectory. if repubish: waypoint_msg = WaypointRoute() waypoint_msg.speed = 2 loc0 = Waypoint() loc0.pose = self.current_pose loc0.nav_type = Waypoint.NAV_STATION loc0.station_duration = -1 waypoint_msg.waypoints = [loc0] self.waypoint_pub.publish(waypoint_msg) if wait: rospy.loginfo("Arrived at target") return
def getStationMsg(self, pose1, pose2, dest_pose, duration, set_yaw): # Calculate angle vessel needs to be at if set_yaw: delta_x = pose2.position.x - pose1.position.x delta_y = pose2.position.y - pose1.position.y yaw = math.atan2(delta_y, delta_x) quat = tf.transformations.quaternion_from_euler(0, 0, yaw) wp_msg = Waypoint() # Waypoint msg for initial station wp_msg.nav_type = Waypoint.NAV_STATION wp_msg.station_duration = duration wp_msg.pose = dest_pose if set_yaw: wp_msg.pose.orientation.x = quat[0] wp_msg.pose.orientation.y = quat[1] wp_msg.pose.orientation.z = quat[2] wp_msg.pose.orientation.w = quat[3] return wp_msg
def objectCB(self, data): verbosePrint("object callback recieved: {}".format(len(data.objects))) # integrate the data into our list self.objectCache = data # if we are still, check what kinds of objects are there and activate curiosity or actual good moving if (self.state == "still"): verbosePrint("still,proceeding") # Find closest unnavigated red and green closest_ranges = { "surmark46104": 1000, "surmark950400": 1000, "surmark950410": 1000 } closest_frames = { "surmark46104": None, "surmark950400": None, "surmark950410": None } for i in data.objects: if not ((i.frame_id in self.addressedState) and (self.addressedState[i.frame_id] == "navigated")): if i.best_guess in closest_ranges: self.tf_listener.waitForTransform( i.frame_id, "base_link", rospy.Time(0), rospy.Duration(self.WAITTIME)) (trans, rot) = self.tf_listener.lookupTransform( i.frame_id, "base_link", rospy.Time(0)) dist = math.sqrt(trans[0] * trans[0] + trans[1] * trans[1]) if dist < closest_ranges[i.best_guess]: closest_ranges[i.best_guess] = dist closest_frames[i.best_guess] = i.frame_id closest_left = None if (closest_ranges["surmark950400"] < closest_ranges["surmark46104"]): closest_left = closest_frames["surmark950400"] else: closest_left = closest_frames["surmark46104"] if (not closest_left is None) and ( not closest_frames["surmark950410"] is None): course = WaypointRoute() self.tf_listener.waitForTransform( closest_left, "map", rospy.Time(0), rospy.Duration(self.WAITTIME)) self.tf_listener.waitForTransform( closest_frames["surmark950410"], "map", rospy.Time(0), rospy.Duration(self.WAITTIME)) (leftPos, rot) = self.tf_listener.lookupTransform( closest_left, "map", rospy.Time(0)) (rightPos, rot) = self.tf_listener.lookupTransform( closest_frames["surmark950410"], "map", rospy.Time(0)) midpoint = Waypoint() midpoint.pose.position.x = (leftPos[0] + rightPos[0]) / 2 midpoint.pose.position.y = (leftPos[1] + rightPos[1]) / 2 course.waypoints.append(midpoint) self.pub.publish(course) self.state = "moving" self.addressedState[closest_left] = "navigated" print("moving between {} and {}".format( closest_left, closest_frames["surmark950410"])) else: verbosePrint("marker find failed, curious mode") # Curiosity # Find closest unknown mindist = 1000 minItem = None for i in data.objects: if (i.best_guess == "buoy"): self.tf_listener.waitForTransform( i.frame_id, "base_link", rospy.Time(0), rospy.Duration(self.WAITTIME)) (trans, rot) = self.tf_listener.lookupTransform( i.frame_id, "base_link", rospy.Time(0)) dist = math.sqrt(trans[0] * trans[0] + trans[1] * trans[1]) if dist < mindist: mindist = dist minItem = i.frame_id if (not minItem is None): print("curiously inspecting {}".format(minItem)) course = WaypointRoute() self.tf_listener.waitForTransform( minItem, "base_link", rospy.Time(0), rospy.Duration(self.WAITTIME)) (relPos, rot) = self.tf_listener.lookupTransform( minItem, "base_link", rospy.Time(0)) self.tf_listener.waitForTransform( "base_link", "map", rospy.Time(0), rospy.Duration(self.WAITTIME)) (basePos, rot) = self.tf_listener.lookupTransform( "base_link", "map", rospy.Time(0)) itemWP = Waypoint() itemWP.pose.position.x = relPos[0] * \ self.CURIO_DIST/mindist + basePos[0] itemWP.pose.position.y = relPos[1] * \ self.CURIO_DIST/mindist + basePos[1] # orientation is also important here so we are looking at the bouy q = quaternion_from_euler(0, 0, math.atan2(relPos[1], relPos[0])) itemWP.pose.orientation.x = q[0] itemWP.pose.orientation.y = q[1] itemWP.pose.orientation.z = q[2] itemWP.pose.orientation.w = q[3] course.waypoints.append(itemWP) self.pub.publish(course) self.state = "moving"
def navigateTo(self, target, wait=True, timeout=0): """ Navigate to the location, if wait is True: Wait until destination is reached, if not, """ rospy.loginfo("Navigating to a location x: %d. y:%d", target.position.x, target.position.y) waypoint_msg = WaypointRoute() waypoint_msg.speed = 2 ##For Now, waypoints are 2 waypoints. At nav waypoint then a nav station loc0 = Waypoint() loc0.pose = self.current_pose loc0.nav_type = Waypoint.NAV_STATION loc0.station_duration = 2 loc1 = Waypoint() loc1.pose = target loc1.nav_type = Waypoint.NAV_WAYPOINT loc2 = Waypoint() loc2.pose = target loc2.nav_type = Waypoint.NAV_STATION loc2.station_duration = -5 waypoint_msg.waypoints=[loc0,loc1,loc2] self.waypoint_pub.publish(waypoint_msg) rate = rospy.Rate(20) while not self.inRange(target): rate.sleep() rospy.loginfo("Arrived at target") rospy.sleep(3) return