def execute_cb(self, goal):
"""This is the callback that will occur when a new goal arrives
through the simple action client. goal will have a
.target_pose field
"""
# Store the goal pose in the global frame so we can
# monitor progress.
goal_pose_world = util.pose_transform(self.tf_buffer,
goal.target_pose, 'map')
path = self.request_plan(goal.target_pose)
if path is None:
msg = "Global planner couldn't find plan. Giving up."
self.action_s.set_aborted(text=msg)
return
# After this happens the local planner should be working.
self.path_pub.publish(path.plan)
at_goal = False
stuck = False
rate = rospy.Rate(10)
poses = deque() # store recent poses to see if we are stuck.
while not at_goal and not stuck and not rospy.is_shutdown():
if self.action_s.is_preempt_requested():
rospy.loginfo('Navigation Preempted')
self.path_pub.publish(Path())
self.action_s.set_preempted()
break
robot_pose = self.get_current_pose()
distance = util.distance(robot_pose, goal_pose_world)
if distance < self.xy_goal_tolerance:
at_goal = True
rospy.loginfo("Goal reached.")
self.action_s.set_succeeded()
elif self._check_stuck(poses, robot_pose):
stuck = True
msg = "Not making progress. Giving up."
self.path_pub.publish(Path())
self.action_s.set_aborted(text=msg)
else:
feedback = MoveBaseFeedback()
feedback.base_position = robot_pose
self.action_s.publish_feedback(feedback)
rate.sleep()
class recv_coords():
feedback = MoveBaseFeedback()
def __init__(self):
feedback = MoveBaseFeedback()
self._as = actionlib.SimpleActionClient('/move_base',MoveBaseAction)
rospy.loginfo('connecting')
self._as.wait_for_server()
rospy.loginfo('server_connected')
self.goal = MoveBaseGoal()
rospy.sleep(0.1)
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.pose.position.z = 0
self.goal.target_pose.pose.orientation.x = 0
self.goal.target_pose.pose.orientation.y = 0
def get_coords(self, coords):
values = coords
while not rospy.is_shutdown():
self.goal.target_pose.pose.position.x = values['position']['x']
self.goal.target_pose.pose.position.y = values['position']['x']
self.goal.target_pose.pose.orientation.z = values['orientation']['z']
self.goal.target_pose.pose.orientation.w = values['orientation']['w']
print(self.goal)
self._as.send_goal(self.goal, feedback_cb=self.feedback_callbak)
self._as.wait_for_result()
rospy.loginfo('moved to location')
def feedback_callbak(self, feedback):
print(feedback)
class W2WMissionPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def __init__(self, name):
self._action_name = name
#self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.planner_as_name = rospy.get_param('~path_planner_as')
self.path_topic = rospy.get_param('~path_topic')
self.wp_topic = rospy.get_param('~wp_topic')
self.map_frame = rospy.get_param('~map_frame', 'map')
# The waypoints as a path
rospy.Subscriber(self.path_topic, Path, self.path_cb, queue_size=1)
self.latest_path = Path()
# LC waypoints, individually
rospy.Subscriber(self.wp_topic, PoseStamped, self.wp_cb, queue_size=1)
# The client to send each wp to the server
self.ac = actionlib.SimpleActionClient(self.planner_as_name,
MoveBaseAction)
while not self.ac.wait_for_server(
rospy.Duration(1)) and not rospy.is_shutdown():
rospy.loginfo("Waiting for action client: %s",
self.planner_as_name)
rospy.loginfo("Action client connected: %s", self.planner_as_name)
while not rospy.is_shutdown():
if self.latest_path.poses:
# Get next waypoint in path
rospy.loginfo("Sending WP")
wp = self.latest_path.poses[0]
del self.latest_path.poses[0]
# TODO: normalize quaternions here according to rviz warning?
goal = MoveBaseGoal(wp)
goal.target_pose.header.frame_id = self.map_frame
self.ac.send_goal(goal)
self.ac.wait_for_result()
rospy.loginfo("WP reached, moving on to next one")
else:
rospy.Rate(1).sleep()
def path_cb(self, path_msg):
self.latest_path = path_msg
rospy.loginfo("Path received with number of wp: %d",
len(self.latest_path.poses))
def wp_cb(self, wp_msg):
# Waypoints for LC from the backseat driver
rospy.loginfo("LC wp received")
self.latest_path.poses.insert(0, wp_msg)
class task4():
feedback = MoveBaseFeedback()
def __init__(self):
self.client = actionlib.SimpleActionClient('/move_base',
MoveBaseAction)
self.pose = PoseWithCovarianceStamped()
rospy.loginfo('wait_for_server')
self.client.wait_for_server()
rospy.loginfo('server found')
self.goal = MoveBaseGoal()
rospy.sleep(0.5)
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.pose.position.z = 0
self.goal.target_pose.pose.orientation.x = 0
self.goal.target_pose.pose.orientation.y = 0
def file_open(self):
os.chdir("/home/user/catkin_ws/src/navigation_exam/params")
paramlist = rosparam.load_file("task2.yaml", default_namespace=None)
for params, ns in paramlist: #ns,param
for key, value in params.items():
rosparam.upload_params(ns, params)
return self.point(key, value)
def point(self, key, val):
if key == "point1":
self.goal.target_pose.pose.position.x = val['position']['x']
self.goal.target_pose.pose.position.y = val['position']['y']
self.goal.target_pose.pose.orientation.z = val['orientation']['z']
self.goal.target_pose.pose.orientation.w = val['orientation']['w']
return self.goal
elif key == "point2":
self.goal.target_pose.pose.position.x = val['position']['x']
self.goal.target_pose.pose.position.y = val['position']['y']
self.goal.target_pose.pose.orientation.z = val['orientation']['z']
self.goal.target_pose.pose.orientation.w = val['orientation']['w']
return self.goal
def callback(self, feedback):
print(feedback)
def sending(self):
while not rospy.is_shutdown():
goal = self.file_open()
print(goal)
self.client.send_goal(goal, feedback_cb=self.callback)
self.client.wait_for_result()
rospy.loginfo('moved to point 1')
goal = self.file_open()
self.client.send_goal(goal, feedback_cb=self.callback)
self.client.wait_for_result()
rospy.loginfo('moved to point 2')
def __init__(self):
self.status=0;
self.last_move_time=time.time();
self.goal_pose=MoveBaseGoal()
self.feedback_pose=MoveBaseFeedback()
self.client_moveBase=actionlib.SimpleActionClient('move_base',MoveBaseAction)
rospy.loginfo("wait waikup move_base")
self.client_moveBase.wait_for_server()
sub_cmd_vel=rospy.Subscriber('cmd_vel',Twist, self.callback)
rospy.loginfo("finish")
def __init__(self):
feedback = MoveBaseFeedback()
self._as = actionlib.SimpleActionClient('/move_base',MoveBaseAction)
rospy.loginfo('connecting')
self._as.wait_for_server()
rospy.loginfo('server_connected')
self.goal = MoveBaseGoal()
rospy.sleep(0.1)
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.pose.position.z = 0
self.goal.target_pose.pose.orientation.x = 0
self.goal.target_pose.pose.orientation.y = 0
def move_base_cb(self, goal):
rospy.loginfo('move_base_cb')
self.target_position = goal.target_pose.pose.position
self.target_orientation = goal.target_pose.pose.orientation
self.moves_base = True
while not self.reply:
rospy.sleep(0.2)
(trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
self.robot_pose_.pose.position.x = trans[0]
self.robot_pose_.pose.position.y = trans[1]
feedback = MoveBaseFeedback()
feedback.base_position.pose.position.x = \
self.robot_pose_.pose.position.x
feedback.base_position.pose.position.y = \
self.robot_pose_.pose.position.y
self.move_base_as_.publish_feedback(feedback)
if self.move_base_as_.is_preempt_requested():
self.preempted += 1
rospy.loginfo("Preempted!")
self.moves_base = False
self.move_base_as_.set_preempted()
return None
if self.move_base_as_.is_new_goal_available():
self.preempted += 1
self.move_base_cb(self.move_base_as_.accept_new_goal())
return None
else:
result = MoveBaseResult()
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
if self.navigation_succedes:
self.move_base_as_.set_succeeded(result)
else:
self.move_base_as_.set_aborted(result)
def execute_callback(self, move_base_goal):
self.goal = move_base_goal.target_pose.pose # Set the provided goal as the current goal
rospy.logdebug('[Move Base Fake] Execute Callback: {}'.format(
str(self.goal.position)))
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
r = rospy.Rate(1)
while not rospy.is_shutdown():
# Always start by checking if there is a new goal that preempts the current one
if self.action_server.is_preempt_requested():
# Tell Bug algorithm to stop before changing or stopping goal
self.pub_state_to_bug(False,
self.bugType) # Stop bug algorithm
if self.action_server.is_new_goal_available():
new_goal = self.action_server.accept_new_goal(
) # There is a new goal
rospy.logdebug('[Move Base Fake] New Goal: {}'.format(
str(self.goal.position)))
self.goal = new_goal.target_pose.pose # Set the provided goal as the current goal
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
else:
self.action_server.set_preempted(
) # No new goal, we've just been told to stop
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
return
# Start goal
if self.valid_goal and not self.current_goal_started:
rospy.logdebug('[Move Base Fake] Starting Goal')
#set the goal
self.pub_goal_to_bug(self.goal, self.bugType)
#start to go to the goal
self.pub_state_to_bug(True, self.bugType)
self.current_goal_started = True # Only start once
# Feedback ever loop just reports current location
feedback = MoveBaseFeedback()
feedback.base_position.pose.position = self.position
self.action_server.publish_feedback(feedback)
# Completed is set in a callback that you need to link to a service or subscriber
if self.current_goal_complete:
rospy.logdebug('[Move Base Fake] Finishing Goal')
#stop the bug algorithm
self.pub_state_to_bug(False, self.bugType)
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
self.action_server.set_succeeded(
MoveBaseResult(), 'Goal reached') # Send success message
return
r.sleep()
# Shutdown
rospy.logdebug('[Move Base Fake] Shutting Down')
self.pub_state_to_bug(False, self.bugType)
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
# to communicate thru ActionLib with MoveBase to go around in a square.
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseFeedback, MoveBaseResult
from math import radians, pi, sqrt, asin, atan2
from tf.transformations import euler_from_quaternion, quaternion_from_euler
routes = [[0.532, 0.0, pi / 2.0], [0.532, 0.5,
pi], [0.032, 0.5, 3.0 * pi / 2.0],
[0.032, 0.0, 0.0]] # go around in a square... anti-cl
rospy.init_node('movebase_client')
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
Feedback = MoveBaseFeedback(
) # ... = Feedback.base_position. header, pose. position, orientation: like PoseStamped
def QtoYaw(orientation_q):
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
if yaw < 0:
yaw = 2 * pi + yaw # goes from 0 to 2*pi, anti-clockwise. jumps at 0:2pi
return yaw
def active_cb():
print("Goal pose is now being processed by the MoveBase Action Server...")
class BackseatDriver(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def __init__(self, name):
self._action_name = name
#self.heading_offset = rospy.get_param('~heading_offsets', 5.)
# self.planner_as_name = rospy.get_param('~path_planner_as')
self.path_topic = rospy.get_param('~path_topic')
self.map_frame = rospy.get_param('~map_frame', 'map')
self.base_frame = rospy.get_param('~base_frame', 'base_link')
self.avg_pose_top = rospy.get_param("~average_pose_topic",
'/average_pose')
self.cov_threshold = rospy.get_param("~cov_threshold", 50)
self.wp_topic = rospy.get_param('~wp_topic')
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
# To send LC wp to the mission planner
self.wp_pub = rospy.Publisher(self.wp_topic, PoseStamped, queue_size=1)
self.listener = tf.TransformListener()
# The waypoints as a path
rospy.Subscriber(self.path_topic, Path, self.path_cb, queue_size=1)
self.latest_path = Path()
# The PF filter state
rospy.Subscriber(self.avg_pose_top,
PoseWithCovarianceStamped,
self.pf_cb,
queue_size=1)
self.closing_loop = False
self.new_wp = PoseStamped()
# The LC waypoints, as a path
self.lc_waypoints = Path()
self.lc_waypoints.header.frame_id = self.map_frame
# One LC wp for testing
lc_wp = PoseStamped()
lc_wp.header.frame_id = self.map_frame
lc_wp.pose.position.x = -183.65
lc_wp.pose.position.y = -332.39
lc_wp.pose.position.z = 0.
lc_wp.pose.orientation.w = 1
self.lc_waypoints.poses.append(lc_wp)
rospy.spin()
def path_cb(self, path_msg):
self.latest_path = path_msg
rospy.loginfo("Path received with number of wp: %d",
len(self.latest_path.poses))
def pf_cb(self, pf_msg):
# Reconstruct covariance
self.cov = np.zeros((6, 6))
for i in range(3):
for j in range(3):
self.cov[i, j] = pf_msg.pose.covariance[i * 3 + j]
# Reconstruct pose estimate
position_estimate = pf_msg.pose.pose.position
# Monitor trace
trc = np.sum(np.diag(self.cov))
if trc > self.cov_threshold and not self.closing_loop:
# Pose uncertainty too high, closing the loop to relocalize
# Find LC wp between current PF pose and next wp on the survey
self.new_wp = self.lc_waypoints.poses[0]
# Preempt current waypoint/path
self.wp_pub.publish(self.new_wp)
rospy.loginfo("Sent LC waypoint")
self.closing_loop = True
elif self.closing_loop:
rospy.loginfo("Going for a loop closure!")
try:
(trans,
rot) = self.listener.lookupTransform(self.map_frame,
self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
start_pos = np.array(trans)
end_pos = np.array([
self.new_wp.pose.position.x, self.new_wp.pose.position.y,
self.new_wp.pose.position.z
])
rospy.loginfo("BS driver diff " +
str(np.linalg.norm(start_pos - end_pos)))
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
# Goal reached
rospy.loginfo("Loop closed!")
self.closing_loop = False
class WPDepthPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def create_marker(self, yaw_setpoint, depth_setpoint):
self.marker.header.frame_id = "/sam/odom"
self.marker.header.stamp = rospy.Time(0)
self.marker.ns = "/sam/viz"
self.marker.id = 0
self.marker.type = 0
self.marker.action = 0
self.marker.pose.position.x = self._feedback.base_position.pose.position.x
self.marker.pose.position.y = self._feedback.base_position.pose.position.y
self.marker.pose.position.z = self._feedback.base_position.pose.position.z
q = quaternion_from_euler(0,0,yaw_setpoint)
self.marker.pose.orientation.x = q[0]
self.marker.pose.orientation.y = q[1]
self.marker.pose.orientation.z = q[2]
self.marker.pose.orientation.w = q[3]
self.marker.scale.x = 1
self.marker.scale.y = 0.1
self.marker.scale.z = 0.1
self.marker.color.a = 1.0 # Dont forget to set the alpha!
self.marker.color.r = 1.0
self.marker.color.g = 1.0
self.marker.color.b = 1.0
self.marker_pub.publish(self.marker)
def yaw_feedback_cb(self,yaw_feedback):
self.yaw_feedback= yaw_feedback.data
def angle_wrap(self,angle):
if(abs(angle)>3.141516):
angle= angle - (abs(angle)/angle)*2*3.141516; #Angle wrapping between -pi and pi
rospy.loginfo_throttle_identical(20, "Angle Error Wrapped")
return angle
def turbo_turn(self,angle_error):
rpm = self.turbo_turn_rpm
rudder_angle = self.rudder_angle
flip_rate = self.flip_rate
left_turn = True
#left turn increases value of yaw angle towards pi, right turn decreases it towards -pi.
if angle_error < 0:
left_turn = False
rospy.loginfo('Right turn!')
rospy.loginfo('Turbo Turning!')
if left_turn:
rudder_angle = -rudder_angle
thrust_rate = 11.
rate = rospy.Rate(thrust_rate)
self.vec_pub.publish(0., rudder_angle, Header())
loop_time = 0.
dualrpm = DualThrusterRPM()
while not rospy.is_shutdown() and loop_time < .37/flip_rate:
dualrpm.thruster_front.rpm = rpm
dualrpm.thruster_back.rpm = rpm
self.rpm_pub.publish(dualrpm)
loop_time += 1./thrust_rate
rate.sleep()
self.vec_pub.publish(0., -rudder_angle, Header())
loop_time = 0.
while not rospy.is_shutdown() and loop_time < .63/flip_rate:
dualrpm.thruster_front.rpm = -rpm
dualrpm.thruster_back.rpm = -rpm
self.rpm_pub.publish(dualrpm)
loop_time += 1./thrust_rate
rate.sleep()
def execute_cb(self, goal):
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
self.nav_goal_frame = goal.target_pose.header.frame_id
if self.nav_goal_frame is None or self.nav_goal_frame == '':
rospy.logwarn("Goal has no frame id! Using utm by default")
self.nav_goal_frame = 'utm'
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(self.nav_goal_frame, goal_point)
self.nav_goal.position.x = goal_point_local.point.x
self.nav_goal.position.y = goal_point_local.point.y
self.nav_goal.position.z = goal_point_local.point.z
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print ("Not transforming point to world local")
pass
rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x)
r = rospy.Rate(11.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
self.yaw_pid_enable.publish(True)
self.depth_pid_enable.publish(True)
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
success = False
self.nav_goal = None
# Stop thrusters
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = 0.
rpm.thruster_back.rpm = 0.
self.rpm_pub.publish(rpm)
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
print('wp depth action planner: stopped thrusters')
self._as.set_preempted(self._result, "Preempted WP action")
return
# Publish feedback
if counter % 5 == 0:
try:
(trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("Error with tf:"+str(self.nav_goal_frame) + " to "+str(self.base_frame))
continue
pose_fb = PoseStamped()
pose_fb.header.frame_id = self.nav_goal_frame
pose_fb.pose.position.x = trans[0]
pose_fb.pose.position.y = trans[1]
pose_fb.pose.position.z = trans[2]
self._feedback.base_position = pose_fb
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._as.publish_feedback(self._feedback)
#rospy.loginfo("Sending feedback")
#Compute yaw setpoint.
xdiff = self.nav_goal.position.x - pose_fb.pose.position.x
ydiff = self.nav_goal.position.y - pose_fb.pose.position.y
yaw_setpoint = math.atan2(ydiff,xdiff)
print('xdiff:',xdiff,'ydiff:',ydiff,'yaw_setpoint:',yaw_setpoint)
#compute yaw_error (e.g. for turbo_turn)
yaw_error= -(self.yaw_feedback - yaw_setpoint)
yaw_error= self.angle_wrap(yaw_error) #wrap angle error between -pi and pi
depth_setpoint = self.nav_goal.position.z
self.depth_pub.publish(depth_setpoint)
#self.vbs_pid_enable.publish(False)
#self.vbs_pub.publish(depth_setpoint)
if self.vel_ctrl_flag:
rospy.loginfo_throttle_identical(5, "vel ctrl, no turbo turn")
#with Velocity control
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
# Publish to velocity controller
self.vel_pid_enable.publish(True)
self.vel_pub.publish(self.vel_setpoint)
self.roll_pub.publish(self.roll_setpoint)
#rospy.loginfo("Velocity published")
else:
if self.turbo_turn_flag:
#if turbo turn is included
rospy.loginfo("Yaw error: %f", yaw_error)
if abs(yaw_error) > self.turbo_angle_min and abs(yaw_error) < self.turbo_angle_max:
#turbo turn with large deviations, maximum deviation is 3.0 radians to prevent problems with discontinuities at +/-pi
self.yaw_pid_enable.publish(False)
self.turbo_turn(yaw_error)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(True)
self.vbs_pub.publish(depth_setpoint)
else:
rospy.loginfo_throttle_identical(5,"Normal WP following")
#normal turning if the deviation is small
self.vbs_pid_enable.publish(False)
self.depth_pid_enable.publish(True)
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
self.create_marker(yaw_setpoint,depth_setpoint)
# Thruster forward
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = self.forward_rpm
rpm.thruster_back.rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
else:
#turbo turn not included, no velocity control
rospy.loginfo_throttle_identical(5, "Normal WP following, no turbo turn")
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
self.create_marker(yaw_setpoint,depth_setpoint)
# Thruster forward
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = self.forward_rpm
rpm.thruster_back.rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
counter += 1
r.sleep()
# Stop thruster
self.vel_pid_enable.publish(False)
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = 0.0
rpm.thruster_back.rpm = 0.0
self.rpm_pub.publish(rpm)
#Stop controllers
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def timer_callback(self, event):
if self.nav_goal is None:
#rospy.loginfo_throttle(30, "Nav goal is None!")
return
try:
(trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
# TODO: we could use this code for the other check also
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
#print("Checking if nav goal is reached!")
start_pos = np.array(trans)
end_pos = np.array([self.nav_goal.position.x, self.nav_goal.position.y, self.nav_goal.position.z])
# We check for success out of the main control loop in case the main control loop is
# running at 300Hz or sth. like that. We dont need to check succes that frequently.
xydiff = start_pos[:2] - end_pos[:2]
zdiff = np.abs(np.abs(start_pos[2]) - np.abs(end_pos[2]))
xydiff_norm = np.linalg.norm(xydiff)
# rospy.logdebug("diff xy:"+ str(xydiff_norm)+' z:' + str(zdiff))
rospy.loginfo("diff xy:"+ str(xydiff_norm)+' z:' + str(zdiff)+ " WP tol:"+ str(self.wp_tolerance)+ "Depth tol:"+str(self.depth_tolerance))
if xydiff_norm < self.wp_tolerance and zdiff < self.depth_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
def __init__(self, name):
"""Publish yaw and depth setpoints based on waypoints"""
self._action_name = name
#self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.wp_tolerance = rospy.get_param('~wp_tolerance', 5.)
self.depth_tolerance = rospy.get_param('~depth_tolerance', 0.5)
self.base_frame = rospy.get_param('~base_frame', "sam/base_link")
rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/thrusters_cmd')
heading_setpoint_topic = rospy.get_param('~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint')
yaw_pid_enable_topic = rospy.get_param('~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable')
depth_setpoint_topic = rospy.get_param('~depth_setpoint_topic', '/sam/ctrl/dynamic_depth/setpoint')
depth_pid_enable_topic = rospy.get_param('~depth_pid_enable_topic', '/sam/ctrl/dynamic_depth/pid_enable')
self.forward_rpm = int(rospy.get_param('~forward_rpm', 1000))
#related to turbo turn
self.turbo_turn_flag = rospy.get_param('~turbo_turn_flag', False)
thrust_vector_cmd_topic = rospy.get_param('~thrust_vector_cmd_topic', '/sam/core/thrust_vector_cmd')
yaw_feedback_topic = rospy.get_param('~yaw_feedback_topic', '/sam/ctrl/yaw_feedback')
self.turbo_angle_min_deg = rospy.get_param('~turbo_angle_min', 90)
self.turbo_angle_min = np.radians(self.turbo_angle_min_deg)
self.turbo_angle_max = 3.0
self.flip_rate = rospy.get_param('~flip_rate', 0.5)
self.rudder_angle = rospy.get_param('~rudder_angle', 0.08)
self.turbo_turn_rpm = rospy.get_param('~turbo_turn_rpm', 1000)
vbs_pid_enable_topic = rospy.get_param('~vbs_pid_enable_topic', '/sam/ctrl/vbs/pid_enable')
vbs_setpoint_topic = rospy.get_param('~vbs_setpoint_topic', '/sam/ctrl/vbs/setpoint')
#related to velocity regulation instead of rpm
self.vel_ctrl_flag = rospy.get_param('~vel_ctrl_flag', False)
self.vel_setpoint = rospy.get_param('~vel_setpoint', 0.5) #velocity setpoint in m/s
self.roll_setpoint = rospy.get_param('~roll_setpoint', 0)
vel_setpoint_topic = rospy.get_param('~vel_setpoint_topic', '/sam/ctrl/dynamic_velocity/u_setpoint')
roll_setpoint_topic = rospy.get_param('~roll_setpoint_topic', '/sam/ctrl/dynamic_velocity/roll_setpoint')
vel_pid_enable_topic = rospy.get_param('~vel_pid_enable_topic', '/sam/ctrl/dynamic_velocity/pid_enable')
self.nav_goal = None
self.listener = tf.TransformListener()
rospy.Timer(rospy.Duration(0.5), self.timer_callback)
self.yaw_feedback=0
rospy.Subscriber(yaw_feedback_topic, Float64, self.yaw_feedback_cb)
self.rpm_pub = rospy.Publisher(rpm_cmd_topic, DualThrusterRPM, queue_size=10)
self.yaw_pub = rospy.Publisher(heading_setpoint_topic, Float64, queue_size=10)
self.depth_pub = rospy.Publisher(depth_setpoint_topic, Float64, queue_size=10)
self.vel_pub = rospy.Publisher(vel_setpoint_topic, Float64, queue_size=10)
self.roll_pub = rospy.Publisher(roll_setpoint_topic, Float64, queue_size=10)
#TODO make proper if it works.
self.vbs_pub = rospy.Publisher(vbs_setpoint_topic, Float64, queue_size=10)
self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic, Bool, queue_size=10)
self.depth_pid_enable = rospy.Publisher(depth_pid_enable_topic, Bool, queue_size=10)
self.vbs_pid_enable = rospy.Publisher(vbs_pid_enable_topic, Bool, queue_size=10)
self.vel_pid_enable = rospy.Publisher(vel_pid_enable_topic, Bool, queue_size=10)
self.vec_pub = rospy.Publisher(thrust_vector_cmd_topic, ThrusterAngles, queue_size=10)
self.marker = Marker()
self.marker_pub = rospy.Publisher('/sam/viz/wp_marker', Marker, queue_size=1)
self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start = False)
self._as.start()
rospy.loginfo("Announced action server with name: %s", self._action_name)
rospy.spin()
class P2PPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
# helper variables
#r = rospy.Rate(1)
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
r = rospy.Rate(10.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
self.nav_goal = None
# Stop thrusters
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = 0.
self.rpm_pub.publish(rpm)
break
# Publish feedback
if counter % 100 == 0:
try:
(trans, rot) = self.listener.lookupTransform(
"/world", "sam/base_link", rospy.Time(0))
pose_fb = PoseStamped()
pose_fb.header.frame_id = "/world"
pose_fb.pose.position.x = trans[0]
pose_fb.pose.position.y = trans[1]
pose_fb.pose.position.z = trans[2]
self._feedback.base_position = pose_fb
self._feedback.base_position.header.stamp = rospy.get_rostime(
)
self._as.publish_feedback(self._feedback)
rospy.loginfo("Sending feedback")
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("Error with tf")
continue
# Thruster forward
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = 1000.
self.rpm_pub.publish(rpm)
rospy.loginfo("Thrusters forward")
counter += 1
r.sleep()
if success:
# Stop thruster
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = -1000.0
cnt = 0
while not rospy.is_shutdown() and cnt < 50:
self.rpm_pub.publish(rpm)
cnt += 1
r.sleep()
#self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def timer_callback(self, event):
if self.nav_goal is None:
rospy.loginfo("Nav goal is None!")
return
try:
(trans,
rot) = self.listener.lookupTransform("/world", self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
# TODO: we could use this code for the other check also
goal_point = PointStamped()
goal_point.header.frame_id = "/world"
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_base = self.listener.transformPoint(
self.base_frame, goal_point)
if goal_point_base.point.x < 0.:
rospy.loginfo("Ahead of goal, returning success!")
self.nav_goal = None
return
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
#print("Checking if nav goal is reached!")
current_pos = np.array(trans)
current_pos[2] = 0.0
end_pos = np.array(
[self.nav_goal.position.x, self.nav_goal.position.y, 0.])
if np.linalg.norm(current_pos - end_pos) < self.goal_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
#else:
# print("Did not reach nav goal!")
def __init__(self, name):
"""Plot an example bezier curve."""
self._action_name = name
self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.base_frame = rospy.get_param('~base_frame',
"lolo_auv_1/base_link")
self.nav_goal = None
self.listener = tf.TransformListener()
rospy.Timer(rospy.Duration(2), self.timer_callback)
self.rpm_pub = rospy.Publisher('/uavcan_rpm_command',
ThrusterRPMs,
queue_size=10)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
r = rospy.Rate(10) # 10hz
rospy.spin()
def compute_relative_target(self):
"""
Compute the target pose in the base_link frame and publish the current pose of the robot
"""
try:
# Get the base_link transformation
(base_position,
base_orientation) = self.transform_listener.lookupTransform(
'/map', '/base_link', rospy.Time())
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return None
# Publish feedback (the current pose)
feedback = MoveBaseFeedback()
feedback.base_position.header.stamp = rospy.Time().now()
feedback.base_position.pose.position.x = base_position[0]
feedback.base_position.pose.position.y = base_position[1]
feedback.base_position.pose.position.z = base_position[2]
feedback.base_position.pose.orientation.x = base_orientation[0]
feedback.base_position.pose.orientation.y = base_orientation[1]
feedback.base_position.pose.orientation.z = base_orientation[2]
feedback.base_position.pose.orientation.w = base_orientation[3]
self.base_position = base_position
self.base_orientation = base_orientation
self._as.publish_feedback(feedback)
# Compute the relative goal position
goal_position_difference = [
self.target_pose.target_pose.pose.position.x -
feedback.base_position.pose.position.x,
self.target_pose.target_pose.pose.position.y -
feedback.base_position.pose.position.y
]
# Get the current orientation and the goal orientation
current_orientation = feedback.base_position.pose.orientation
p = [current_orientation.x, current_orientation.y, current_orientation.z, \
current_orientation.w]
goal_orientation = self.target_pose.target_pose.pose.orientation
q = [goal_orientation.x, goal_orientation.y, goal_orientation.z, \
goal_orientation.w]
# Rotate the relative goal position into the base frame
goal_position_base_frame = tf.transformations.quaternion_multiply(
tf.transformations.quaternion_inverse(p),
tf.transformations.quaternion_multiply([
goal_position_difference[0], goal_position_difference[1], 0, 0
], p))
# Compute the difference to the goal orientation
orientation_to_target = tf.transformations.quaternion_multiply(q, \
tf.transformations.quaternion_inverse(p))
yaw = tf.transformations.euler_from_quaternion(
orientation_to_target)[2]
rel_target = PoseStamped()
rel_target.pose.position.x = goal_position_base_frame[0]
rel_target.pose.position.y = -goal_position_base_frame[1]
rel_target.pose.orientation.z = yaw
self.relative_target_pub.publish(rel_target)
return (goal_position_base_frame[0], -goal_position_base_frame[1], yaw)
def follow_path(self, path):
for pose in path.plan.poses:
print("Going to", pose.pose.position.x, pose.pose.position.y)
self.turtle.set_pos(pose.pose.position.x, pose.pose.position.y)
self.server.publish_feedback(MoveBaseFeedback(base_position=pose))
self.turtle.stop()
class LeaderFollower(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
rospy.loginfo_throttle(5, "Goal received")
success = True
rate = rospy.Rate(11.) # 10hz
counter = 0
while not rospy.is_shutdown():
self.yaw_pid_enable.publish(True)
self.depth_pid_enable.publish(True)
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
success = False
# Stop thrusters
self.rpm.thruster_1_rpm = 0.
self.rpm.thruster_2_rpm = 0.
self.rpm_pub.publish(self.rpm)
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
print('leader_follower_action: stopped thrusters')
self._as.set_preempted(self._result, "Preempted WP action")
return
# Compute and Publish setpoints
if counter % 1 == 0:
# distance check is done in the BT, we will add CBFs here later, which will include
# that distance as a constraint anyways
# if sqrt(rel_trans[0]**2 + rel_trans[1]**2 + rel_trans[2]**2) < self.min_dist:
# break
#Check transform between SAM1 (leader- goal) and SAM2 (follower -self), define a goal point and call the go_to_point() function
self.leader_frame = goal.target_pose.header.frame_id
try:
(follower_trans,
follower_rot) = self.listener.lookupTransform(
self.follower_odom, self.follower_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logwarn_throttle_identical(
5, "Could not get transform between " +
self.leader_frame + " and " + self.follower_frame)
success = False
break
except:
rospy.logwarn_throttle_identical(
5, "Could not do tf lookup for some other reason")
success = False
break
try:
(leader_trans, leader_rot) = self.listener.lookupTransform(
self.follower_odom, self.leader_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logwarn_throttle_identical(
5, "Could not get transform between " +
self.leader_frame + " and " + self.follower_frame)
success = False
break
except:
rospy.logwarn_throttle_identical(
5, "Could not do tf lookup for some other reason")
success = False
break
xdiff = leader_trans[0] - follower_trans[0]
ydiff = leader_trans[1] - follower_trans[1]
zdiff = leader_trans[2] - follower_trans[2]
yaw_setpoint = math.atan2(ydiff, xdiff)
depth_setpoint = -zdiff
rospy.loginfo_throttle(
10, 'yaw_setpoint:' + str(yaw_setpoint) +
' depth_setpoint:' + str(depth_setpoint))
self.depth_pub.publish(depth_setpoint)
if self.vel_ctrl_flag:
rospy.loginfo_throttle_identical(
5, "vel ctrl, no turbo turn")
#with Velocity control
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
# Publish to velocity controller
self.vel_pid_enable.publish(True)
self.vel_pub.publish(self.vel_setpoint)
self.roll_pub.publish(self.roll_setpoint)
#rospy.loginfo("Velocity published")
else:
#turbo turn not included, no velocity control
rospy.loginfo_throttle_identical(
5, "Normal WP following, no turbo turn")
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
# Thruster forward
self.rpm.thruster_1_rpm = self.forward_rpm
self.rpm.thruster_2_rpm = self.forward_rpm
self.rpm_pub.publish(self.rpm)
#rospy.loginfo("Thrusters forward")
counter += 1
rate.sleep()
# Stop thruster
self.vel_pid_enable.publish(False)
self.rpm.thruster_1_rpm = 0.0
self.rpm.thruster_2_rpm = 0.0
self.rpm_pub.publish(self.rpm)
#Stop controllers
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
if self._result:
rospy.loginfo('%s: Succeeded' % self._action_name)
else:
rospy.logwarn_throttle_identical(3,
'%s: Failed' % self._action_name)
self._as.set_succeeded(self._result)
def __init__(self, name):
self.rpm = ThrusterRPMs()
"""Publish yaw and depth setpoints based on waypoints"""
self._action_name = name
self.follower_frame = rospy.get_param('~follower_frame',
'/sam_2/base_link')
self.follower_odom = rospy.get_param('~follower_odom', '/sam_2/odom')
# self.min_dist = rospy.get_param('~min_dist', 5.)
rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/rpm_cmd')
heading_setpoint_topic = rospy.get_param(
'~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint')
yaw_pid_enable_topic = rospy.get_param(
'~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable')
depth_setpoint_topic = rospy.get_param(
'~depth_setpoint_topic', '/sam/ctrl/dynamic_depth/setpoint')
depth_pid_enable_topic = rospy.get_param(
'~depth_pid_enable_topic', '/sam/ctrl/dynamic_depth/pid_enable')
self.forward_rpm = int(rospy.get_param('~forward_rpm', 1000))
#related to velocity regulation instead of rpm
self.vel_ctrl_flag = rospy.get_param('~vel_ctrl_flag', False)
self.vel_setpoint = rospy.get_param('~vel_setpoint',
0.5) #velocity setpoint in m/s
self.roll_setpoint = rospy.get_param('~roll_setpoint', 0)
vel_setpoint_topic = rospy.get_param(
'~vel_setpoint_topic', '/sam/ctrl/dynamic_velocity/u_setpoint')
roll_setpoint_topic = rospy.get_param(
'~roll_setpoint_topic', '/sam/ctrl/dynamic_velocity/roll_setpoint')
vel_pid_enable_topic = rospy.get_param(
'~vel_pid_enable_topic', '/sam/ctrl/dynamic_velocity/pid_enable')
self.listener = tf.TransformListener()
self.rpm_pub = rospy.Publisher(rpm_cmd_topic,
ThrusterRPMs,
queue_size=10)
self.yaw_pub = rospy.Publisher(heading_setpoint_topic,
Float64,
queue_size=10)
self.depth_pub = rospy.Publisher(depth_setpoint_topic,
Float64,
queue_size=10)
self.vel_pub = rospy.Publisher(vel_setpoint_topic,
Float64,
queue_size=10)
self.roll_pub = rospy.Publisher(roll_setpoint_topic,
Float64,
queue_size=10)
self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic,
Bool,
queue_size=10)
self.depth_pid_enable = rospy.Publisher(depth_pid_enable_topic,
Bool,
queue_size=10)
self.vel_pid_enable = rospy.Publisher(vel_pid_enable_topic,
Bool,
queue_size=10)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
rospy.spin()
class W2WPathPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
self.nav_goal_frame = goal.target_pose.header.frame_id
if self.nav_goal_frame is None or self.nav_goal_frame == '':
rospy.logwarn("Goal has no frame id! Using map by default")
self.nav_goal_frame = self.map_frame
r = rospy.Rate(10.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
self.nav_goal = None
# Stop thruster
self.motion_command(0., 0., 0.)
break
# Transform goal map --> base frame
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(
self.base_frame, goal_point)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("Not transforming point to base frame")
pass
#Compute throttle error
# throttle_level = min(self.max_throttle, np.linalg.norm(
# np.array([goal_point_local.point.x + goal_point_local.point.y])))
# Nacho: no real need to adjust the throttle
throttle_level = self.max_throttle
# Compute thrust error
alpha = math.atan2(goal_point_local.point.y,
goal_point_local.point.x)
sign = np.copysign(1, alpha)
yaw_setpoint = sign * min(self.max_thrust, abs(alpha))
# Command velocities
self.motion_command(throttle_level, yaw_setpoint, 0.)
# Publish feedback
if counter % 10 == 0:
self._as.publish_feedback(self._feedback)
counter += 1
r.sleep()
# Stop thruster
self.motion_command(0., 0., 0.)
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def motion_command(self, throttle_level, thruster_angle,
inclination_angle):
incl = Float64()
throttle = Float64()
thrust = Float64()
throttle.data = throttle_level
thrust.data = thruster_angle
incl.data = inclination_angle
self.thruster_pub.publish(thrust)
self.inclination_pub.publish(incl)
self.throttle_pub.publish(throttle)
def timer_callback(self, event):
if self.nav_goal is None:
#rospy.loginfo_throttle(30, "Nav goal is None!")
return
# Check if the goal has been reached
try:
(trans,
rot) = self.listener.lookupTransform(self.nav_goal_frame,
self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
start_pos = np.array(trans)
end_pos = np.array([
self.nav_goal.position.x, self.nav_goal.position.y,
self.nav_goal.position.z
])
rospy.logdebug("diff " + str(np.linalg.norm(start_pos - end_pos)))
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
# Goal reached
self.nav_goal = None
def __init__(self, name):
self._action_name = name
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.max_throttle = rospy.get_param('~max_throttle', 2.)
self.max_thrust = rospy.get_param('~max_thrust', 0.5)
self.map_frame = rospy.get_param('~map_frame', 'map')
self.base_frame = rospy.get_param('~base_frame', 'base_link')
self.odom_frame = rospy.get_param('~odom_frame', 'odom')
self.throttle_top = rospy.get_param('~throttle_cmd', '/throttle')
self.thruster_top = rospy.get_param('~thruster_cmd', '/thruster')
self.inclination_top = rospy.get_param('~inclination_cmd',
'/inclination')
self.as_name = rospy.get_param('~path_planner_as', 'path_planner')
self.nav_goal = None
self.listener = tf.TransformListener()
rospy.Timer(rospy.Duration(2), self.timer_callback)
self.throttle_pub = rospy.Publisher(self.throttle_top,
Float64,
queue_size=1)
self.thruster_pub = rospy.Publisher(self.thruster_top,
Float64,
queue_size=1)
self.inclination_pub = rospy.Publisher(self.inclination_top,
Float64,
queue_size=1)
self._as = actionlib.SimpleActionServer(self.as_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s", self.as_name)
rospy.spin()
class YawPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
self.nav_goal_frame = goal.target_pose.header.frame_id
if self.nav_goal_frame is None or self.nav_goal_frame == '':
rospy.logwarn("Goal has no frame id! Using utm by default")
self.nav_goal_frame = '/utm'
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(
self.nav_goal_frame, goal_point)
self.nav_goal.position.x = goal_point_local.point.x
self.nav_goal.position.y = goal_point_local.point.y
self.nav_goal.position.z = goal_point_local.point.z
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("Not transforming point to world local")
pass
rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x)
r = rospy.Rate(11.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
self.yaw_pid_enable.publish(True)
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
self.nav_goal = None
# Stop thrusters
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = 0.
rpm.thruster_2_rpm = 0.
self.rpm_pub.publish(rpm)
self.yaw_pid_enable.publish(False)
break
# Publish feedback
if counter % 10 == 0:
try:
(trans, rot) = self.listener.lookupTransform(
self.nav_goal_frame, self.base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("Error with tf:" + str(self.nav_goal_frame) +
" to " + str(self.base_frame))
continue
pose_fb = PoseStamped()
pose_fb.header.frame_id = self.nav_goal_frame
pose_fb.pose.position.x = trans[0]
pose_fb.pose.position.y = trans[1]
pose_fb.pose.position.z = trans[2]
self._feedback.base_position = pose_fb
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._as.publish_feedback(self._feedback)
#rospy.loginfo("Sending feedback")
#Compute yaw setpoint.
xdiff = self.nav_goal.position.x - pose_fb.pose.position.x
ydiff = self.nav_goal.position.y - pose_fb.pose.position.y
yaw_setpoint = math.atan2(ydiff, xdiff)
self.yaw_pub.publish(yaw_setpoint)
#rospy.loginfo("Yaw setpoint: %f", yaw_setpoint)
# Thruster forward
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = self.forward_rpm
rpm.thruster_2_rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
counter += 1
r.sleep()
# Stop thruster
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = 0.0
rpm.thruster_2_rpm = 0.0
self.rpm_pub.publish(rpm)
#Stop yaw controller
self.yaw_pid_enable.publish(False)
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def timer_callback(self, event):
if self.nav_goal is None:
#rospy.loginfo_throttle(30, "Nav goal is None!")
return
try:
(trans,
rot) = self.listener.lookupTransform(self.nav_goal_frame,
self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
# TODO: we could use this code for the other check also
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
#print("Checking if nav goal is reached!")
start_pos = np.array(trans)
end_pos = np.array([
self.nav_goal.position.x, self.nav_goal.position.y,
self.nav_goal.position.z
])
rospy.loginfo("diff " + str(np.linalg.norm(start_pos - end_pos)))
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
def __init__(self, name):
"""Publish yaw setpoints based on waypoints"""
self._action_name = name
#self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.base_frame = rospy.get_param('~base_frame', "sam/base_link")
self.utm_frame = rospy.get_param('~utm_frame', "utm")
rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/rpm_cmd')
heading_setpoint_topic = rospy.get_param(
'~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint')
yaw_pid_enable_topic = rospy.get_param(
'~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable')
self.forward_rpm = int(rospy.get_param('~forward_rpm', 700))
self.nav_goal = None
self.listener = tf.TransformListener()
rospy.Timer(rospy.Duration(2), self.timer_callback)
self.rpm_pub = rospy.Publisher(rpm_cmd_topic,
ThrusterRPMs,
queue_size=10)
self.yaw_pub = rospy.Publisher(heading_setpoint_topic,
Float64,
queue_size=10)
self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic,
Bool,
queue_size=10)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
rospy.spin()
class BezierPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def callback(self, pose_msg):
self.nav_goal = pose_msg.pose
path, pose = self.plan()
self.pub.publish(path)
def execute_cb(self, goal):
# helper variables
#r = rospy.Rate(1)
success = True
self.nav_goal = goal.target_pose.pose
# append the seeds for the fibonacci sequence
#self._feedback.base_position.header.frame_id = "/world"
# publish info to the console for the user
#rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))
r = rospy.Rate(0.1) # 10hz
while not rospy.is_shutdown() and self.nav_goal is not None:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
self.nav_goal = None
break
path, pose = self.plan()
self.pub.publish(path)
self._feedback.base_position = pose
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._as.publish_feedback(self._feedback)
r.sleep()
self.pub.publish(Path())
if success:
#self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def plan(self):
try:
(trans,
rot) = self.listener.lookupTransform("/world", self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return Path(), PoseStamped()
start_pos = np.array(trans)
euler = tf.transformations.euler_from_quaternion(rot)
start_pitch = euler[1] #np.radians(-40.0) # [rad]
start_yaw = euler[2] # np.radians(180.0) # [rad]
end_pos = np.array(
[self.nav_goal.position.x, self.nav_goal.position.y, -85.])
#if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
# rospy.loginfo("Reached goal!")
# self.nav_goal = None
# return Path()
end_rot = [
self.nav_goal.orientation.x, self.nav_goal.orientation.y,
self.nav_goal.orientation.z, self.nav_goal.orientation.w
]
euler = tf.transformations.euler_from_quaternion(end_rot)
end_pitch = euler[1]
end_yaw = euler[2]
curve, control_points = calc_4points_bezier_path(
start_pos,
start_yaw,
start_pitch,
end_pos,
end_yaw,
end_pitch,
self.heading_offset,
n_points=self.n_points)
#derivatives_cp = bezier_derivatives_control_points(control_points, 1)
path = Path()
path.header.frame_id = "/world"
path.header.stamp = rospy.get_rostime()
for i in range(0, self.n_points):
pose = PoseStamped()
pose.pose.position.x = curve.T[0][i]
pose.pose.position.y = curve.T[1][i]
pose.pose.position.z = curve.T[2][i]
path.poses.append(pose)
pose = PoseStamped()
pose.pose.position.x = start_pos[0]
pose.pose.position.y = start_pos[1]
pose.pose.position.z = start_pos[2]
return path, pose
def timer_callback(self, event):
if self.nav_goal is None:
#print("Nav goal is None!")
return
try:
(trans,
rot) = self.listener.lookupTransform("/world", self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
#print("Checking if nav goal is reached!")
start_pos = np.array(trans)
end_pos = np.array(
[self.nav_goal.position.x, self.nav_goal.position.y, -85.])
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
#else:
# print("Did not reach nav goal!")
def __init__(self, name):
"""Plot an example bezier curve."""
self._action_name = name
self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.n_points = rospy.get_param('~number_points', 100)
self.base_frame = rospy.get_param('~base_frame',
"lolo_auv_1/base_link")
self.nav_goal = None
self.listener = tf.TransformListener()
self.pub = rospy.Publisher('/global_plan', Path, queue_size=10)
rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.callback)
rospy.Timer(rospy.Duration(1), self.timer_callback)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
r = rospy.Rate(0.1) # 10hz
while not rospy.is_shutdown():
if self.nav_goal is not None:
path, pose = self.plan()
self.pub.publish(path)
r.sleep()
class BezierPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
success = True
self.nav_goal = goal.target_pose.pose
# Target in word_utm coord, translate to world_local
goal_point = PointStamped()
goal_point.header.frame_id = "/world_utm"
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(
"/world_local", goal_point)
self.nav_goal.position.x = goal_point_local.point.x
self.nav_goal.position.y = goal_point_local.point.y
self.nav_goal.position.z = goal_point_local.point.z
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("Not transforming point to world local")
pass
rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x)
r = rospy.Rate(10.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
self.nav_goal = None
break
if counter % 100 == 0:
path, pose = self.plan()
self.pub.publish(path)
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._feedback.base_position.header.frame_id = "/world_utm"
self._feedback.base_position = pose
self._as.publish_feedback(self._feedback)
counter += 1
r.sleep()
self.pub.publish(Path())
if success:
#self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def plan(self):
try:
(trans,
rot) = self.listener.lookupTransform("/world_local",
self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return Path(), PoseStamped()
start_pos = np.array(trans)
start_rot = np.array(rot)
euler = tf.transformations.euler_from_quaternion(rot)
start_pitch = euler[1] #np.radians(-40.0) # [rad]
start_yaw = euler[2] # np.radians(180.0) # [rad]
end_pos = np.array([
self.nav_goal.position.x, self.nav_goal.position.y,
self.nav_goal.position.z
])
end_rot = [
self.nav_goal.orientation.x, self.nav_goal.orientation.y,
self.nav_goal.orientation.z, self.nav_goal.orientation.w
]
euler = tf.transformations.euler_from_quaternion(end_rot)
end_pitch = euler[1]
end_yaw = euler[2]
curve, control_points = calc_4points_bezier_path(
start_pos,
start_yaw,
start_pitch,
end_pos,
end_yaw,
end_pitch,
self.heading_offset,
n_points=self.n_points)
#derivatives_cp = bezier_derivatives_control_points(control_points, 1)
path = Path()
path.header.frame_id = "/world_local"
path.header.stamp = rospy.get_rostime()
for i in range(0, self.n_points):
pose = PoseStamped()
pose.pose.position.x = curve.T[0][i]
pose.pose.position.y = curve.T[1][i]
pose.pose.position.z = curve.T[2][i]
path.poses.append(pose)
# Revert pose to UTM coordinates for feedback to the BT
pose = PoseStamped()
pose.header.frame_id = "/world_local"
pose.pose.position.x = start_pos[0]
pose.pose.position.y = start_pos[1]
pose.pose.position.z = start_pos[2]
pose.pose.orientation.x = start_rot[0]
pose.pose.orientation.y = start_rot[1]
pose.pose.orientation.z = start_rot[2]
pose.pose.orientation.w = start_rot[3]
print("orientation")
print(start_rot)
try:
pose_local = self.listener.transformPose("/world_utm", pose)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
return path, pose_local
def timer_callback(self, event):
if self.nav_goal is None:
# print("Nav goal is None!")
return
try:
(trans,
rot) = self.listener.lookupTransform("/world", self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
goal_point = PointStamped()
goal_point.header.frame_id = "/world"
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
# try:
# goal_point_base = self.listener.transformPoint(self.base_frame, goal_point)
# if goal_point_base.point.x < 0.:
# rospy.loginfo("Ahead of goal, returning success!")
# self.nav_goal = None
# return
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# pass
#print("Checking if nav goal is reached!")
start_pos = np.array(trans)
end_pos = np.array([
self.nav_goal.position.x, self.nav_goal.position.y,
self.nav_goal.position.z
])
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
#else:
# print("Did not reach nav goal!")
def __init__(self, name):
"""Plot an example bezier curve."""
self._action_name = name
self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.n_points = rospy.get_param('~number_points', 100)
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.nav_goal = None
self.listener = tf.TransformListener()
self.pub = rospy.Publisher('/global_plan', Path, queue_size=10)
rospy.Timer(rospy.Duration(0.1), self.timer_callback)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
r = rospy.Rate(10) # 10hz
counter = 0
while not rospy.is_shutdown():
if counter % 100 == 0 and self.nav_goal is not None:
path, pose = self.plan()
self.pub.publish(path)
r.sleep()
counter += 1