def __init__(self, nodename, target):
MoveBaseUtil.__init__(self, nodename)
# set the distance between waypoints
self.geo = {}
self.target_lat = rospy.get_param("~latitude", target[0])
self.target_lon = rospy.get_param("~longitude", target[1])
self.geo["goal_heading"] = rospy.get_param("~heading", target[2])
# self.geo["waypoint_distance"] = rospy.get_param("~waypoint_distance", waypoint_distance)
rate = rospy.Rate(10)
self.fix_received = False
rospy.wait_for_message("/navsat/fix", NavSatFix)
rospy.Subscriber("/navsat/fix",
NavSatFix,
self.navsat_fix_callback,
queue_size=50)
while not self.fix_received:
rospy.sleep(1)
waypoint = self.create_waypoint()
# rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
# self.move_base.wait_for_server(rospy.Duration(60))
# rospy.loginfo("Connected to move base server")
# rospy.loginfo("Starting navigation test")
# Update the marker display
self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoint
# Start the robot moving toward the goal
self.move(goal, mode=0, mode_param=3)
def __init__(self,
nodename,
quadrant=1,
map_length=10,
map_width=10,
half_period=10,
half_amplitude=10,
offset=0):
MoveBaseUtil.__init__(self, nodename)
self.quadrant = rospy.get_param("~quadrant", quadrant)
self.map_length = rospy.get_param("~map_length", map_length)
self.map_width = rospy.get_param("~map_width", map_width)
self.map_half_period = rospy.get_param("~half_period", half_period)
self.map_half_amplitude = rospy.get_param("~half_amplitude",
half_amplitude)
self.map_offset = rospy.get_param("~offset", offset)
# get boat position, one time only
self.odom_received = False
rospy.wait_for_message("/odom", Odometry)
rospy.Subscriber("/odom", Odometry, self.odom_callback, queue_size=50)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
# # information about map, length (X), width (Y), position of the initial point
# self.map_length = map_length
# self.map_width = map_width
# # set the half period and half amplitude
# map_half_period = half_period
# map_half_amplitude = half_amplitude
# map_offset = offset
while not self.odom_received:
rospy.sleep(1)
# assumption point 0,0 is the left-bottom of map
if self.quadrant == 1:
# create waypoints for quadrant 1
waypoints = self.create_waypoints(
self.map_half_period, self.map_half_amplitude, self.map_offset,
self.map_length / 2, self.map_width / 2, self.map_length / 2,
self.map_width / 2)
elif self.quadrant == 2:
# create waypoints for quadrant 2
waypoints = self.create_waypoints(
self.map_half_period, self.map_half_amplitude, self.map_offset,
self.map_length / 2, self.map_width / 2, 0, self.map_width / 2)
elif self.quadrant == 3:
# create waypoints for quadrant 3
waypoints = self.create_waypoints(self.map_half_period,
self.map_half_amplitude,
self.map_offset,
self.map_length / 2,
self.map_width / 2, 0, 0)
elif self.quadrant == 4:
# create waypoints for quadrant 4
waypoints = self.create_waypoints(self.map_half_period,
self.map_half_amplitude,
self.map_offset,
self.map_length / 2,
self.map_width / 2,
self.map_length / 2, 0)
else: # e.g. 0
# create waypoints for the whole map
waypoints = self.create_waypoints(self.map_half_period,
self.map_half_amplitude,
self.map_offset, self.map_length,
self.map_width, 0, 0)
# Initialize the visualization markers for RViz
self.init_markers()
# Set a visualization marker at each waypoint
for waypoint in waypoints:
p = Point()
p = waypoint.position
self.markers.points.append(p)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 0
# Cycle through the waypoints
while i < len(waypoints) and not rospy.is_shutdown():
# Update the marker display
self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoints[i]
# Start the robot moving toward the goal
self.move(goal, 1, 2)
i += 1
else: # escape constant forward and continue to the next waypoint
pass
def __init__(self, nodename):
MoveBaseUtil.__init__(self, nodename)
# get boat position, one time only
self.odom_received = False
rospy.wait_for_message("/odom", Odometry)
rospy.Subscriber("/odom", Odometry, self.odom_callback, queue_size=50)
# information about map, length (X), width (Y), position of the center wrt boat: boat-center
# TODO (1) map center to absolute
# (2) get parameters by rospy.get_param()
self.map_length = rospy.get_param("~length", 20)
self.map_width = rospy.get_param("~width", 20)
self.map_center_x = rospy.get_param("~center_x", 10)
self.map_center_y = rospy.get_param("~center_y", 10)
# set the offset distance from border
self.map_offset = rospy.get_param("~offset", 3)
# self.map_info = {"l":self.map_length, "w":self.map_width,
# "center_x":self.map_center_x, "center_y":self.map_center_y}
while not self.odom_received:
rospy.sleep(1)
# create waypoints
waypoints = self.scout_waypoints(self.map_length, self.map_width,
self.map_center_x, self.map_center_y,
self.map_offset)
# Set a visualization marker at each waypoint
for waypoint in waypoints:
p = Point()
p = waypoint.position
self.markers.points.append(p)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 0
# Cycle through the four waypoints
while i < len(waypoints) and not rospy.is_shutdown():
# Update the marker display
self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoints[i]
# Start the robot moving toward the goal
self.move(goal, 1, 2)
i += 1
else: # escape constant forward and continue to the next waypoint
rospy.loginfo("Navigation test finished.")
pass
def __init__(self, nodename, target=[10,1.57,0], radius=5, duration=10):
MoveBaseUtil.__init__(self, nodename)
self.target = Twist(Point(rospy.get_param("~target_x", target[0]),
rospy.get_param("~target_y"), target[1]),
Point(0, 0, rospy.get_param("~angle", target[2])))
self.radius = rospy.get_param("~radius", radius)
self.duration = ospy.get_param("~duration", duration)
#get boat pose one time only
self.odom_received = False
rospy.wait_for_message("/odom", Odometry)
rospy.Subscriber("/odom", Odometry, self.odom_callback, queue_size = 50)
while not self.odom_received:
rospy.sleep(1)
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
q_angle = quaternion_from_euler(0, 0, self.target.angular.z)
angle = Quaternion(*q_angle)
station=Pose(self.target.linear, angle)
p = Point()
p = station.position
self.markers.points.append(p)
self.marker_pub.publish(self.markers)
#get start time
start_time= rospy.get_time()
while ((rospy.get_time()-start_time < self.duration) or not self.duration) and not rospy.is_shutdown():
if (sqrt((self.target.linear.x-self.x0)**2 + (self.target.linear.y-self.y0)**2) < self.radius):
self.cmd_vel_pub.publish(Twist())
rospy.loginfo("inside inner radius, no action")
else:
rospy.loginfo("outside radius")
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the waypoint
goal.target_pose.pose = station
# Start the robot moving toward the goal
self.move(goal, 0, 0)
rospy.loginfo("goal sent")
def __init__(self,
nodename,
target=[10, 0, 0],
radius=5,
polygon=6,
is_ccw=True,
is_relative=True):
MoveBaseUtil.__init__(self, nodename)
self.loiter = {}
self.mode = rospy.get_param("~mode", 1)
self.mode_param = rospy.get_param("~mode_param", 2)
self.target = Point(rospy.get_param("~target_x", target[0]),
rospy.get_param("~target_y", target[1]), 0)
self.loiter["radius"] = rospy.get_param("~radius", radius) #double
self.loiter["polygon"] = rospy.get_param("~polygon", polygon) #int
self.loiter["is_ccw"] = rospy.get_param("~is_ccw", is_ccw) #bool
self.loiter["is_relative"] = rospy.get_param("~is_relative",
is_relative) #bool
# find the target
if self.loiter["is_relative"]:
self.loiter["center"], self.loiter["heading"] = \
self.convert_relative_to_absolute([self.target.x, self.target.y])
else: # absolute
# obtained from vision nodes, absolute catersian
# but may be updated later, so need to callback
self.loiter["center"] = (self.target.x, self.target.y,
self.target.z) # (x, y, 0)
# heading from boat to center
self.loiter["heading"] = atan2(self.target.y - self.y0,
self.target.x - self.x0)
# create waypoints
waypoints = self.create_waypoints()
# # Initialize the visualization markers for RViz
# self.init_markers()
# Set a visualization marker at each waypoint
for waypoint in waypoints:
p = Point()
p = waypoint.position
self.markers.points.append(p)
# # Subscribe to the move_base action server
# self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# rospy.loginfo("Waiting for move_base action server...")
# # Wait 60 seconds for the action server to become available
# self.move_base.wait_for_server(rospy.Duration(60))
# rospy.loginfo("Connected to move base server")
# rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 0
# Cycle through the waypoints
while i <= self.loiter["polygon"] and not rospy.is_shutdown():
# Update the marker display
self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoints[i]
# Start the robot moving toward the goal
self.move(goal, self.mode, self.mode_param)
i += 1
else: # escape loiter and continue to the next waypoint
print "task finished"
def __init__(self, nodename, target=[20,0,0], waypoint_separation=5, is_relative=True):
MoveBaseUtil.__init__(self, nodename)
self.forward = {}
self.target = Point(rospy.get_param("~target_x", target[0]), rospy.get_param("~target_y", target[1]), 0.0)
self.mode = rospy.get_param("~mode", 0)
self.mode_param = rospy.get_param("~mode_param", 1)
self.forward["waypoint_separation"] = rospy.get_param("~waypoint_separation", waypoint_separation)
self.forward["is_relative"] = rospy.get_param("~is_relative", is_relative)
# # get boat position, one time only
# self.odom_received = False
# rospy.wait_for_message("/odom", Odometry)
# rospy.Subscriber("/odom", Odometry, self.odom_callback, queue_size=50)
# while not self.odom_received:
# rospy.sleep(1)
# # set the distance between waypoints
# self.forward["waypoint_separation"] = waypoint_separation
# # check whether absolute or relative target
# self.forward["is_relative"] = is_relative
if self.forward["is_relative"]:
self.forward["translation"], self.forward["heading"] = self.convert_relative_to_absolute([self.target.x, self.target.y])
# print self.forward["translation"], self.forward["heading"]
self.forward["goal_distance"] = self.target.x
else:
# obtained from vision nodes, absolute catersian
# but may be updated later, so need to callback
self.forward["translation"] = (self.target.x, self.target.y, self.target.z) # (x, y, 0)
self.forward["goal_distance"] = sqrt((self.target.x - self.x0) ** 2 + (self.target.y - self.y0) ** 2)
# heading from boat to center
self.forward["heading"] = atan2(self.target.y - self.y0, self.target.x - self.x0)
# create waypoints
waypoints = self.create_waypoints()
# Initialize the visualization markers for RViz
# self.init_markers()
# Set a visualization marker at each waypoint
for waypoint in waypoints:
p = Point()
p = waypoint.position
self.markers.points.append(p)
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
# self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
# self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
# self.move_base.wait_for_server(rospy.Duration(60))
# rospy.loginfo("Connected to move base server")
# rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 1 # remove the first point
# Cycle through the four waypoints
while i < len(waypoints) and not rospy.is_shutdown():
# Update the marker display
self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoints[i]
# Start the robot moving toward the goal
self.move(goal, self.mode, self.mode_param)
i += 1
else: # escape constant forward and continue to the next waypoint
print "task finished"
def __init__(self, nodename, target=[10, 1.57, 0], is_relative=True):
MoveBaseUtil.__init__(self, nodename)
self.target = Point(rospy.get_param("~target_x", target[0]),
rospy.get_param("~target_y", target[1]), 0.0)
self.is_relative = rospy.get_param("~is_relative", is_relative)
self.odom_received = False
rospy.wait_for_message("/odometry/filtered/global", Odometry)
rospy.Subscriber("/odometry/filtered/global",
Odometry,
self.odom_callback,
queue_size=50)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
while not self.odom_received:
rospy.sleep(1)
if self.is_relative:
position, heading = self.convert_relative_to_absolute(
[self.x0, self.y0, self.yaw0], [self.target.x, self.target.y])
x, y, _ = position
else:
x, y = self.target.x, self.target.y
q_angle = quaternion_from_euler(0, 0, atan2(y - self.y0, x - self.x0))
angle = Quaternion(*q_angle)
waypoint = Pose(Point(x, y, 0), angle)
# Set a visualization marker at each waypoint
p = Point()
p = waypoint.position
self.markers.points.append(p)
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoint
# Start the robot moving toward the goal
self.move(goal, 0, 0)