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