def transform_pose(self, msg):
# **Assuming /tf2 topic is being broadcasted
tf_buffer = Buffer()
listener = TransformListener(tf_buffer)
pose_cov_stamped = PoseWithCovarianceStamped()
pose_cov_stamped.pose = msg.pose
pose_cov_stamped.header.frame_id = from_frame
pose_cov_stamped.header.stamp = rospy.Time.now()
try:
# ** It is important to wait for the listener to start listening. Hence the rospy.Duration(1)
output_pose_stamped = tf_buffer.transform(pose_cov_stamped, to_frame, rospy.Duration(1))
return output_pose_stamped.pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
raise
class TFManager(object):
"""
Listens to datum GPS or service and broadcasts:
1. ECEF TO UTM
2. UTM TO MAP
3. MAP TO ODOM (fixed or updated by a pose with covariance stamped topic).
"""
def __init__(self):
self._datum = rospy.get_param("~datum", None)
if self._datum is not None and len(self._datum) < 3:
self._datum += [0.0]
self._use_datum = self._datum is not None
self._earth_frame_id = rospy.get_param("~earth_frame_id", "earth")
self._utm_frame_id = rospy.get_param("~utm_frame_id", "utm")
self._map_frame_id = rospy.get_param("~map_frame_id", "map")
self._odom_frame_id = rospy.get_param("~odom_frame_id", "odom")
self._body_frame_id = rospy.get_param("~base_frame_id",
"base_link") # currently unused
self._tf_broadcast_rate = rospy.get_param("~broadcast_rate", 10.0)
self._serv = rospy.Service("~set_datum", SetDatum, self._set_datum)
self._tf_buff = Buffer()
TransformListener(self._tf_buff)
self._tf_bcast = TransformBroadcaster()
self._last_datum = None
self._static_map_odom = rospy.get_param("~static_map_odom", False)
self._odom_updated = False
self._update_odom_serv = rospy.Service("~set_odom", Trigger,
self._handle_set_odom)
if not self._use_datum:
rospy.Subscriber("gps_datum", NavSatFix, self._handle_datum)
if not self._static_map_odom:
rospy.Subscriber("waterlinked/pose_with_cov_stamped",
PoseWithCovarianceStamped, self._handle_odom_tf)
else:
StaticTransformBroadcaster().sendTransform(
TransformStamped(
Header(0, rospy.Time.now(), self._map_frame_id),
self._odom_frame_id, None, Quaternion(0, 0, 0, 1)))
self._map_odom_tf = None
rospy.Timer(rospy.Duration.from_sec(1.0 / self._tf_broadcast_rate),
self._send_tf)
def _handle_set_odom(self, req):
self._odom_updated = False
res = TriggerResponse(True, "map -> odom tf set.")
return res
def _handle_odom_tf(self, msg):
# Given the pose of the vehicle in waterlinked frame, output the map -> odom tf transformation
if not self._odom_updated:
point = PointStamped(
Header(0, rospy.Time.from_sec(0.0), msg.header.frame_id),
msg.pose.pose.position)
try:
point_map = self._tf_buff.transform(point, self._map_frame_id)
except Exception as e:
rospy.logerr_throttle(
5, "{} | {}".format(rospy.get_name(), e.message))
return
# Odom is always at same depth as map
self._map_odom_tf = TransformStamped(
Header(0, rospy.Time.now(), self._map_frame_id),
self._odom_frame_id,
Transform(Vector3(point_map.point.x, point_map.point.y, 0),
Quaternion(0, 0, 0, 1)))
self._odom_updated = True
def _set_datum(self, req):
self._datum = [
req.geo_pose.position.latitude, req.geo_pose.position.longitude,
req.geo_pose.position.altitude
]
return
def _get_coords(self, latitude, longitude):
# Get ECEF translation to UTM and Rotation to UTM from latitude and longitude (assuming 0.0 altitude)
x, y, z = geodetic_helpers.utm_origin_ecef(
*geodetic_helpers.lla_to_ecef(latitude, longitude))
q = geodetic_helpers.latlong_ecef_enu_rotation(latitude, longitude)
# Get UTM translation to ENU and rotation to ENU from latitude and longitude (assuming 0.0 altitude)
utm_current = geodetic_helpers.lla_to_utm(latitude, longitude)
Y = geodetic_helpers.grid_convergence(latitude,
longitude,
radians=False)
Y = 0 # The grid convergence seems to be already accounted for?
enu_rotation = Rotation.from_euler('xyz', [0.0, 0.0, -Y],
degrees=True).as_quat().tolist()
return (x, y, z) + tuple(q), utm_current[:3] + tuple(enu_rotation)
def _get_tfs(self, data):
if type(data) is NavSatFix:
earth, utm = self._get_coords(data.latitude, data.longitude)
header_earth = Header(data.header.seq, data.header.stamp,
self._earth_frame_id)
header_utm = Header(data.header.seq, data.header.stamp,
self._utm_frame_id)
else:
earth, utm = self._get_coords(self._datum[0], self._datum[1])
header_earth = Header(0, rospy.Time.now(), self._earth_frame_id)
header_utm = Header(0, rospy.Time.now(), self._utm_frame_id)
# Broadcast datum's latitude and longitude
# Map from ECEF Frame
earth_to_utm = TransformStamped(
header_earth, self._utm_frame_id,
Transform(Vector3(*earth[:3]), Quaternion(*earth[3:])))
# UTM from ECEF Frame
utm_to_map = TransformStamped(
header_utm, self._map_frame_id,
Transform(Vector3(*utm[:3]), Quaternion(*utm[3:])))
return earth_to_utm, utm_to_map
def _handle_datum(self, msg):
self._last_datum = msg
def _send_tf(self, event):
if self._use_datum:
tf_utm, tf_map = self._get_tfs(None)
elif self._last_datum is not None:
tf_utm, tf_map = self._get_tfs(self._last_datum)
else:
return
self._tf_bcast.sendTransform(tf_utm)
self._tf_bcast.sendTransform(tf_map)
if self._map_odom_tf is not None:
tf = self._map_odom_tf
tf.header.stamp = rospy.Time.now()
self._tf_bcast.sendTransform(tf)
class SimpleCascade(object):
"""
Nothing fancy here, just a 4 DoF (Surge, Sway, Heave, Yaw)
Cascade Controller Can be Configured for Different Modes of Control
Modes
AccelTeleop -> Acceleration Setpoint Given by AccelStamped Message [X]
VelTeleop -> Velocity Setpoint Given by Vector3Stamped Message [X]
HoldPosition -> Position Setpoint Set to Latest Odometry Message [ ]
LOSPosition -> Position Setpoint Set to Requested PointStamped Message [ ]
Autopilot -> Velocity Setpoint Set to Requested Vector3Stamped Message [ ]
Outputs WrenchStamped Message
Requirements
"""
def __init__(self):
self._ready = False
self._first = True
self._mode = ControlMode()
self._mode.mode = self._mode.OFF
self._use_accel_fb = rospy.get_param("~use_accel_fb", default=False)
#-------- TF Buffer ----------
self._tf_buff = Buffer()
TransformListener(self._tf_buff)
#-------- Service Advertisements ---------
self._set_control_mode = rospy.Service("controller/set_control_mode",
SetControlMode,
self._handle_control_mode)
self._configure_mavros = rospy.ServiceProxy("mavros/set_mode", SetMode)
#-------- State Config -------------------
self._control_mode_pub = rospy.Publisher("controller/mode",
ControlMode,
queue_size=5)
self._latest_odom_fb = None
rospy.Subscriber("odometry/filtered", Odometry, self._odom_feedback)
#-------- Wrench Config ------------------
self._wrench = WrenchStamped()
self._wrench_pub = rospy.Publisher("wrench/target",
WrenchStamped,
queue_size=5)
#-------- Acceleration Config ------------
self._latest_accel_sp = None
rospy.Subscriber("accel/setpoint", AccelStamped, self._accel_sp)
self._latest_accel_fb = None
rospy.Subscriber("accel/filtered", AccelWithCovarianceStamped,
self._accel_feedback)
# If using acceleration feedback, then setup the PID Gains and subscribe to the acceleration feedback
if self._use_accel_fb:
# Can only use ax, ay, az because ar is not provided by robot_localization
self._accel_pids = MIMOPID([0, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0], [0, 0, 0, 0])
# Otherwise, mass and moments of inertia must be given
else:
self.mass = rospy.get_param("~pid/mass")
self.inertial = rospy.get_param("~pid/inertial")
# update mass, moments of inertia
self.inertial_tensor = np.array([[
self.inertial['ixx'], self.inertial['ixy'],
self.inertial['ixz']
],
[
self.inertial['ixy'],
self.inertial['iyy'],
self.inertial['iyz']
],
[
self.inertial['ixz'],
self.inertial['iyz'],
self.inertial['izz']
]])
self.mass_inertial_matrix = np.vstack(
(np.hstack((self.mass * np.identity(3), np.zeros((3, 3)))),
np.hstack((np.zeros((3, 3)), self.inertial_tensor))))
#--------- Velocity Config ----------
self._vel_pids = MIMOPID([0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0])
rospy.Subscriber("vel/setpoint", TwistStamped, self._vel_sp)
self._vel_limits = np.zeros((4, 1), dtype=float)
self._latest_vel_sp = None
self._vel_deadband = rospy.get_param("~pid/deadband", 0.01)
#---------- Position Config ---------
self._pos_pids = MIMOPID([0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0],
[0, 0, 0, 0])
rospy.Subscriber("pos/setpoint", PoseStamped, self._pos_sp)
self._latest_pos_sp = None
#---------- Autopilot Config ----------
rospy.Subscriber("autopilot/setpoint", Autopilot, self._ap_sp)
self._latest_ap_sp = None
rospy.Subscriber("range", Range, self._range_feedback)
self._latest_range_fb = None
self._control_depth = True
self._control_sway = True
self._control_yaw = True
#---------- Waypoint Following Config ----------
self._los_server = actionlib.SimpleActionServer(
'follow_waypoints', FollowWaypointsAction, self._los_action_cb,
False)
self._los_server.start()
self._latest_wp = None
# -------- Dynamic Reconfigure Server -------
self._reconfig_serv = Server(pid_reconfigConfig, self._reconfig)
#---------- Timer Loops -----------
rospy.Timer(rospy.Duration.from_sec(1.0 / 10.0), self._main_loop)
self._ready = True
def _reconfig(self, config, level):
if self._use_accel_fb:
self._accel_pids.set_p([
config["accel_x_kp"], config["accel_y_kp"],
config["accel_z_kp"], config["accel_r_kp"]
])
self._accel_pids.set_i([
config["accel_x_ki"], config["accel_y_ki"],
config["accel_z_ki"], config["accel_r_ki"]
])
self._accel_pids.set_d([
config["accel_x_kd"], config["accel_y_kd"],
config["accel_z_kd"], config["accel_r_kd"]
])
self._accel_pids.set_sat([
config["accel_x_sat"], config["accel_y_sat"],
config["accel_z_sat"], config["accel_r_sat"]
])
self._vel_pids.set_p([
config["vel_x_kp"], config["vel_y_kp"], config["vel_z_kp"],
config["vel_r_kp"]
])
self._vel_pids.set_i([
config["vel_x_ki"], config["vel_y_ki"], config["vel_z_ki"],
config["vel_r_ki"]
])
self._vel_pids.set_d([
config["vel_x_kd"], config["vel_y_kd"], config["vel_z_kd"],
config["vel_r_kd"]
])
self._vel_pids.set_sat([
config["vel_x_sat"], config["vel_y_sat"], config["vel_z_sat"],
config["vel_r_sat"]
])
self._vel_limits = np.array(
[[config["vel_x_lim"]], [config["vel_y_lim"]],
[config["vel_z_lim"]], [config["vel_r_lim"]]],
dtype=float)
self._pos_pids.set_p([
config["pos_x_kp"], config["pos_y_kp"], config["pos_z_kp"],
config["pos_r_kp"]
])
self._pos_pids.set_i([
config["pos_x_ki"], config["pos_y_ki"], config["pos_z_ki"],
config["pos_r_ki"]
])
self._pos_pids.set_d([
config["pos_x_kd"], config["pos_y_kd"], config["pos_z_kd"],
config["pos_r_kd"]
])
self._pos_pids.set_sat([
config["pos_x_sat"], config["pos_y_sat"], config["pos_z_sat"],
config["pos_r_sat"]
])
return config
# ------- CONTROL MODE SERVICE --------
def _handle_control_mode(self, mode):
"""
Handles the "set_contol_mode" service.
When called, places the FCU into manual flight mode and clears the last accel, vel, and pos setpoints.
"""
res = SetControlModeResponse()
res.success = False
if not self._ready:
return res
self._mode = mode.mode
req = SetModeRequest()
req.base_mode = 0
#req.custom_mode = "19" # MANUAL FLIGHT MODE
req.custom_mode = "0" # STABILIZE FLIGHT MODE
# req.custom_mode = "2" # ALT_HOLD FLIGHT MODE
self._configure_mavros.wait_for_service()
res.success = self._configure_mavros(req).mode_sent
self._control_depth = True
self._control_sway = True
self._control_yaw = True
self._latest_accel_sp = None
self._latest_vel_sp = None
self._latest_pos_sp = None
self._latest_ap_sp = None
self._latest_wp = None
return res
# ------- EXTERNAL SETPOINT HANDLERS --------
def _vel_sp(self, msg):
if not self._ready:
return
if self._mode.mode == self._mode.VELTELEOP:
self._latest_vel_sp = msg
def _accel_sp(self, msg):
if not self._ready:
return
if self._mode.mode == self._mode.ACCELTELEOP:
self._latest_accel_sp = msg
def _pos_sp(self, msg):
if not self._ready:
return
self._latest_pos_sp = msg
def _ap_sp(self, msg):
if not self._ready:
return
self._latest_ap_sp = msg
# ------- FEEDBACK HANDLERS --------
def _odom_feedback(self, msg):
if not self._ready:
return
self._latest_odom_fb = msg
def _accel_feedback(self, msg):
if not self._ready:
return
self._latest_accel_fb = msg
def _range_feedback(self, msg):
if not self._ready:
return
self._latest_range_fb = msg
# -------- ACTION CALLBACKS --------
def _los_action_cb(self, goal):
r = rospy.Rate(1.0)
results = FollowWaypointsResult()
feedback = FollowWaypointsFeedback()
while self._mode.mode != ControlMode.LOSGUIDANCE:
rospy.logwarn_throttle(
10.0,
"{} | Waypoints pending. Set to LOSGUIDANCE mode to execute.".
format(rospy.get_name()))
if self._los_server.is_preempt_requested():
results.waypoints_completed = 0
self._los_server.set_preempted(results)
return
r.sleep()
rospy.loginfo("{} | Adding {} waypoint commands to Autopilot.".format(
rospy.get_name(), len(goal.waypoints.waypoints)))
for i, wp in enumerate(goal.waypoints.waypoints):
# Get sequential waypoints, and set as latest wp
self._latest_wp = wp
while not self._calc_3D_distance() < wp.radius_of_acceptance:
feedback.percentage_complete = float(i) / float(
len(goal.waypoints.waypoints)) * 100.0
self._los_server.publish_feedback(feedback)
if self._los_server.is_preempt_requested():
self._los_server.set_preempted(results)
return
if self._mode.mode != ControlMode.LOSGUIDANCE:
self._los_server.set_aborted(results)
return
r.sleep()
results.waypoints_completed = i + 1
rospy.loginfo("{} | Waypoint tasks completed.".format(
rospy.get_name()))
self._los_server.set_succeeded(results)
# -------- CONTROL CALCULATIONS -------
def _calc_surface(self):
cmd_accel = AccelStamped(Header(0, rospy.Time.now(), ""), None)
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0,
"{} | SURFACING! No odom feedback.".format(rospy.get_name()))
cmd_accel.accel.linear.z = 10
else:
cmd_accel.accel.linear.z = 10 if self._latest_odom_fb.pose.pose.position.z < -0.5 else 0
self._latest_accel_sp = cmd_accel
return True
def _hold_pos(self):
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No odom feedback.".format(rospy.get_name()))
return False
cmd_ap = Autopilot()
cmd_ap.U = 0
cmd_ap.Z = self._latest_odom_fb.pose.pose.position.z
euler = Rotation.from_quat([
self._latest_odom_fb.pose.pose.orientation.x,
self._latest_odom_fb.pose.pose.orientation.y,
self._latest_odom_fb.pose.pose.orientation.z,
self._latest_odom_fb.pose.pose.orientation.w
]).as_euler("xyz")
cmd_ap.psi = euler[-1]
cmd_ap.reference = cmd_ap.DEPTH
self._control_yaw = False
self._latest_ap_sp = cmd_ap
return True
def _calc_footprint_distance(self):
if self._latest_wp is None:
rospy.logwarn_throttle(
10.0, "{} | No waypoint provided.".format(rospy.get_name()))
return False
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No odom feedback.".format(rospy.get_name()))
return False
"""
Calculate euclidean distance from waypoint to vehicle's footprint here
"""
p = PointStamped(
Header(0, rospy.Time.from_sec(0), self._latest_wp.header.frame_id),
self._latest_wp.point)
if self._latest_wp.header.frame_id != self._latest_odom_fb.header.frame_id:
if self._tf_buff.can_transform(
p.header.frame_id, self._latest_odom_fb.header.frame_id,
rospy.Time.from_sec(0), rospy.Duration(5)):
p = self._tf_buff.transform(
p, self._latest_odom_fb.header.frame_id)
else:
rospy.logwarn_throttle(
10.0, "{} | cannot TF waypoint frame_id: {}".format(
rospy.get_name(), p.header.frame_id))
return False
dx = self._latest_odom_fb.pose.pose.position.x - p.point.x
dy = self._latest_odom_fb.pose.pose.position.y - p.point.y
return np.sqrt(dx**2 + dy**2)
def _calc_3D_distance(self):
if self._latest_wp is None:
rospy.logwarn_throttle(
10.0, "{} | No waypoint provided.".format(rospy.get_name()))
return False
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No odom feedback.".format(rospy.get_name()))
return False
"""
Calculate euclidean distance from waypoint to vehicle's 3D position here
"""
p = PointStamped(
Header(0, rospy.Time.from_sec(0), self._latest_wp.header.frame_id),
self._latest_wp.point)
if self._tf_buff.can_transform(p.header.frame_id,
self._latest_odom_fb.header.frame_id,
rospy.Time.from_sec(0),
rospy.Duration(5)):
p = self._tf_buff.transform(p,
self._latest_odom_fb.header.frame_id)
else:
rospy.logwarn_throttle(
10.0, "{} | cannot TF waypoint frame_id: {}".format(
rospy.get_name(), p.header.frame_id))
return False
dx = self._latest_odom_fb.pose.pose.position.x - p.point.x
dy = self._latest_odom_fb.pose.pose.position.y - p.point.y
if p.point.z <= 0:
dz = self._latest_odom_fb.pose.pose.position.z - p.point.z
else:
dz = self._latest_range_fb.range - p.point.z
return np.sqrt(dx**2 + dy**2 + dz**2)
def _calc_los(self):
"""
Calculate autopilot setpoints
"""
if self._latest_wp is None:
rospy.logwarn_throttle(
10.0, "{} | No waypoint provided.".format(rospy.get_name()))
return False
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No odom feedback.".format(rospy.get_name()))
return False
ap = Autopilot()
ap.Z = self._latest_wp.point.z
ap.U = self._latest_wp.max_forward_speed if self._calc_footprint_distance(
) >= self._latest_wp.radius_of_acceptance else 0
ap.reference = ap.DEPTH if ap.Z <= 0.0 else ap.ALT
p = PointStamped(
Header(0, rospy.Time.from_sec(0), self._latest_wp.header.frame_id),
self._latest_wp.point)
if self._latest_wp.header.frame_id != self._latest_odom_fb.header.frame_id:
if self._tf_buff.can_transform(
p.header.frame_id, self._latest_odom_fb.header.frame_id,
rospy.Time.from_sec(0), rospy.Duration(5)):
p = self._tf_buff.transform(
p, self._latest_odom_fb.header.frame_id)
else:
rospy.logwarn_throttle(
10.0, "{} | cannot TF waypoint frame_id: {}".format(
rospy.get_name(), p.header.frame_id))
return False
ap.psi = atan2(p.point.y - self._latest_odom_fb.pose.pose.position.y,
p.point.x - self._latest_odom_fb.pose.pose.position.x)
ap.header.stamp = rospy.Time.now()
self._latest_ap_sp = ap
return True
def _calc_autopilot_vel(self):
"""
Calculate Velocity Setpoints from U (m/s), z (m) and psi (rad)
Z may be depth, height referenced, or if None will be ignored. Default is depth.
"""
if self._latest_ap_sp is None:
rospy.logwarn_throttle(
10.0, "{} | No autopilot setpoint.".format(rospy.get_name()))
return False
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No odom feedback.".format(rospy.get_name()))
return False
cmd_vel = TwistStamped(
Header(0, rospy.Time.now(), self._latest_odom_fb.child_frame_id),
Twist(None, None))
cmd_vel.twist.linear.x = self._latest_ap_sp.U
euler_error = np.array(
[0, 0, self._latest_ap_sp.psi]) - Rotation.from_quat([
self._latest_odom_fb.pose.pose.orientation.x,
self._latest_odom_fb.pose.pose.orientation.y,
self._latest_odom_fb.pose.pose.orientation.z,
self._latest_odom_fb.pose.pose.orientation.w
]).as_euler("xyz")
euler_error = np.where(
np.bitwise_and(np.abs(euler_error) > np.pi, euler_error < 0),
euler_error + 2 * np.pi, euler_error)
euler_error = np.where(
np.bitwise_and(np.abs(euler_error) > np.pi, euler_error > 0),
euler_error - 2 * np.pi, euler_error)
# TODO this gets set to None if load_waypoints gets called too quickly
if self._latest_ap_sp is None:
return False
if self._latest_ap_sp.reference == self._latest_ap_sp.DEPTH:
z_error = self._latest_ap_sp.Z - self._latest_odom_fb.pose.pose.position.z
self._control_depth = True
elif self._latest_ap_sp.reference == self._latest_ap_sp.ALT:
if self._latest_range_fb is None:
rospy.logerr_throttle(
5, "{} | {}".format(
rospy.get_name(),
"No ranging info available for altitude control."))
return False
z_error = self._latest_ap_sp.Z - self._latest_range_fb.range
self._control_depth = True
elif self._latest_ap_sp.reference == self._latest_ap_sp.NONE:
z_error = 0
self._control_depth = False
else:
rospy.logerr_throttle(
5, "{} | {}".format(rospy.get_name(),
"Autopilot unknown mode."))
return False
pos_err = np.array([[0], [0], [z_error], [euler_error[-1]]],
dtype=float)
_, _, cmd_vel.twist.linear.z, cmd_vel.twist.angular.z = self._pos_pids(
pos_err,
rospy.Time.now().to_sec())
self._control_sway = False
self._latest_vel_sp = cmd_vel
return True
# REGULATE POSITION WITH VELOCITY
def _calc_vel(self):
"""
Calculate Velocity Setpoint from Pose Setpoint PID
"""
if self._latest_pos_sp is None:
rospy.logwarn_throttle(
10.0, "{} | No pos setpoint.".format(rospy.get_name()))
return False
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No pos feedback.".format(rospy.get_name()))
return False
# Placing the setpoint into body-fixed coordinates (similar to Vessel Parallel Transform)
cmd_pos = self._latest_pos_sp
cmd_pos.header.stamp = rospy.Time.from_sec(
0.0) # Get the latest available transform
body_pose = self._tf_buff.transform(
cmd_pos, self._latest_odom_fb.child_frame_id)
euler_error = Rotation.from_quat([
body_pose.pose.orientation.x, body_pose.pose.orientation.y,
body_pose.pose.orientation.z, body_pose.pose.orientation.w
]).as_euler("xyz")
euler_error = np.where(
np.bitwise_and(np.abs(euler_error) > np.pi, euler_error < 0),
euler_error + 2 * np.pi, euler_error)
euler_error = np.where(
np.bitwise_and(np.abs(euler_error) > np.pi, euler_error > 0),
euler_error - 2 * np.pi, euler_error)
pos_err = np.array(
[[body_pose.pose.position.x], [body_pose.pose.position.y],
[body_pose.pose.position.z], [euler_error[-1]]],
dtype=float)
vel_sp_msg = TwistStamped()
vel_sp_msg.header.frame_id = self._latest_odom_fb.child_frame_id
vel_sp_msg.twist.linear.x, vel_sp_msg.twist.linear.y, vel_sp_msg.twist.linear.z, vel_sp_msg.twist.angular.z = np.clip(
self._vel_pids(pos_err,
self._latest_odom_fb.header.stamp.to_sec()).reshape(
4, 1), -self._vel_limits, self._vel_limits)
self._latest_vel_sp = vel_sp_msg
return True
def _enforce_deadband(self, vector):
vector.x = 0 if np.abs(vector.x) < self._vel_deadband else vector.x
vector.y = 0 if np.abs(vector.y) < self._vel_deadband else vector.y
vector.z = 0 if np.abs(vector.z) < self._vel_deadband else vector.z
return vector
# REGULATE VELOCITY WITH ACCELERATION
def _calc_accel(self):
"""
Calculate Acceleration Setpoint from Twist Setpoint PID
"""
if self._latest_vel_sp is None:
rospy.logwarn_throttle(
10.0, "{} | No vel setpoint.".format(rospy.get_name()))
return False
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No vel feedback.".format(rospy.get_name()))
return False
# Convert setpoint velocity into the correct frame
if self._latest_vel_sp.header.frame_id != self._latest_odom_fb.child_frame_id and len(
self._latest_vel_sp.header.frame_id) > 0:
if self._tf_buff.can_transform(self._latest_odom_fb.child_frame_id,
self._latest_vel_sp.header.frame_id,
rospy.Time.from_sec(0.0)):
cmd_vel = TwistStamped()
header = Header(self._latest_vel_sp.header.seq,
rospy.Time.from_sec(0.0),
self._latest_vel_sp.header.frame_id)
cmd_vel.twist.linear = self._tf_buff.transform(
Vector3Stamped(header, self._latest_vel_sp.twist.linear),
self._latest_odom_fb.child_frame_id,
rospy.Duration(5)).vector
cmd_vel.twist.angular = self._tf_buff.transform(
Vector3Stamped(header, self._latest_vel_sp.twist.angular),
self._latest_odom_fb.child_frame_id,
rospy.Duration(5)).vector
cmd_vel.header.stamp = rospy.Time.now()
else:
rospy.logwarn_throttle(
5, "{} | Cannot TF Velocity SP frame_id: {}".format(
rospy.get_name(), self._latest_vel_sp.header.frame_id))
return False
else:
cmd_vel = self._latest_vel_sp
clean_linear_vel = self._enforce_deadband(
self._latest_odom_fb.twist.twist.linear)
clean_angular_vel = self._enforce_deadband(
self._latest_odom_fb.twist.twist.angular)
vel_err = np.array([[cmd_vel.twist.linear.x - clean_linear_vel.x],
[cmd_vel.twist.linear.y - clean_linear_vel.y],
[cmd_vel.twist.linear.z - clean_linear_vel.z],
[
cmd_vel.twist.angular.z -
self._latest_odom_fb.twist.twist.angular.z
]],
dtype=float)
accel_sp_msg = AccelStamped()
accel_sp_msg.header = cmd_vel.header
accel_sp_msg.accel.linear.x, accel_sp_msg.accel.linear.y, accel_sp_msg.accel.linear.z, accel_sp_msg.accel.angular.z = self._vel_pids(
vel_err, self._latest_odom_fb.header.stamp.to_sec())
accel_sp_msg.accel.linear.y = 0 if not self._control_sway else accel_sp_msg.accel.linear.y
accel_sp_msg.accel.linear.z = 0 if not self._control_depth else accel_sp_msg.accel.linear.z
accel_sp_msg.accel.angular.z = 0 if not self._control_yaw else accel_sp_msg.accel.angular.z
self._latest_accel_sp = accel_sp_msg
return True
def _calc_wrench(self):
"""
Calculate Wrench Setpoint from Accel Message
Uses the odometry child_frame_id as the body-fixed frame (ASSUMED FLU)
Will transform the requested setpoint into the body-fixed frame.
"""
if self._latest_accel_sp is None:
rospy.logwarn_throttle(
10.0, "{} | No accel setpoint.".format(rospy.get_name()))
return
if self._latest_accel_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No accel feedback.".format(rospy.get_name()))
return
# Convert setpoint acceleration into the correct frame
if self._latest_accel_sp.header.frame_id != self._latest_accel_fb.header.frame_id and len(
self._latest_accel_sp.header.frame_id) > 0:
if self._tf_buff.can_transform(
self._latest_accel_fb.header.frame_id,
self._latest_accel_sp.header.frame_id,
rospy.Time.from_sec(0.0), rospy.Duration(5)):
header = Header(self._latest_accel_sp.header.seq,
rospy.Time.from_sec(0.0),
self._latest_accel_sp.header.frame_id)
cmd_accel = AccelStamped()
cmd_accel.accel.linear = self._tf_buff.transform(
Vector3Stamped(header, self._latest_accel_sp.accel.linear),
self._latest_accel_fb.header.frame_id,
rospy.Duration(5)).vector
cmd_accel.accel.angular = self._tf_buff.transform(
Vector3Stamped(header,
self._latest_accel_sp.accel.angular),
self._latest_accel_fb.header.frame_id,
rospy.Duration(5)).vector
else:
rospy.logwarn_throttle(
5, "{} | Cannot TF Acceleration SP frame_id: {}".format(
rospy.get_name(),
self._latest_accel_sp.header.frame_id))
return False
else:
cmd_accel = self._latest_accel_sp # If it's already in the correct frame then never mind.
if self._use_accel_fb:
accel_err = np.array(
[[
cmd_accel.accel.linear.x -
self._latest_accel_fb.accel.accel.linear.x
],
[
cmd_accel.accel.linear.y -
self._latest_accel_fb.accel.accel.linear.y
],
[
cmd_accel.accel.linear.z -
self._latest_accel_fb.accel.accel.linear.z
],
[
cmd_accel.accel.angular.z -
self._latest_accel_fb.accel.accel.angular.z
]],
dtype=float)
tau = self._accel_pids(accel_err,
self._latest_accel_fb.header.stamp.to_sec())
else:
accel = np.array([[cmd_accel.accel.linear.x],
[cmd_accel.accel.linear.y],
[cmd_accel.accel.linear.z],
[cmd_accel.accel.angular.x],
[cmd_accel.accel.angular.y],
[cmd_accel.accel.angular.z]])
tau = self.mass_inertial_matrix.dot(accel)[[0, 1, 2, -1], :]
self._wrench.header.stamp = rospy.Time.now()
self._wrench.header.frame_id = cmd_accel.header.frame_id
self._wrench.wrench.force.x, self._wrench.wrench.force.y, self._wrench.wrench.force.z, self._wrench.wrench.torque.z = tau.squeeze(
)
self._wrench_pub.publish(self._wrench)
def _main_loop(self, event):
if not self._ready:
return
self._control_mode_pub.publish(
self._mode) # send the controller's state
if self._mode.mode == self._mode.OFF:
return
elif self._mode.mode == self._mode.IDLE:
return
elif self._mode.mode == self._mode.ACCELTELEOP:
self._calc_wrench()
elif self._mode.mode == self._mode.VELTELEOP:
if self._calc_accel():
self._calc_wrench()
elif self._mode.mode == self._mode.HOLDPOSITION:
if self._hold_pos():
if self._calc_autopilot_vel():
if self._calc_accel():
self._calc_wrench()
elif self._mode.mode == self._mode.AUTOPILOT:
# Given heading and forward velocity, match it!
if self._calc_autopilot_vel():
if self._calc_accel():
self._calc_wrench()
elif self._mode.mode == self._mode.LOSGUIDANCE:
if self._calc_los():
if self._calc_autopilot_vel():
if self._calc_accel():
self._calc_wrench()
elif self._mode.mode == self._mode.ABORT:
if self._calc_surface():
self._calc_wrench()
else:
rospy.logerr_throttle(
5.0, "{} | Control mode {} not implemented.".format(
rospy.get_name(), self._mode.mode))
class ImcInterface(object):
def __init__(self):
self.STATE = PlanControlState.NONE
self._plan_db = dict()
self._current_plan = PlanDB()
datum = rospy.get_param("~datum", [])
datum = map(
float,
datum.strip('][').split(',')) if type(datum) is str else datum
self._datum = None if len(datum) == 0 else GeoPoint(*map(float, datum))
rospy.loginfo("Datum Reference: " + str(self._datum))
self._estimated_state_pub = rospy.Publisher("imc/estimated_state",
EstimatedState,
queue_size=10)
self._estimated_state_msg = EstimatedState()
self._plan_control_state_pub = rospy.Publisher(
"imc/plan_control_state", PlanControlState, queue_size=10)
self._plan_control_state_msg = PlanControlState()
self._plan_control_state_msg.state = self.STATE
self._plan_db_pub = rospy.Publisher("imc/plan_db",
PlanDB,
queue_size=10)
self._geo_converter = rospy.ServiceProxy("convert_points",
ConvertGeoPoints)
self._waypoint_setter = rospy.ServiceProxy("start_waypoint_list",
InitWaypointSet)
self._waypoint_forwarder = rospy.Service("load_waypoints",
InitWaypointSet,
self._send_waypoints)
self._hold_position = rospy.ServiceProxy("hold_vehicle", Hold)
self._tf_buffer = Buffer()
TransformListener(self._tf_buffer)
rospy.Subscriber("gps", NavSatFix, self._handle_gps)
rospy.Subscriber("pose_gt", Odometry, self._handle_pose)
rospy.Subscriber("imc/goto_waypoint", Pose, self._handle_goto)
rospy.Subscriber("imc/plan_control", PlanControl,
self._handle_plan_control)
rospy.Subscriber("imc/abort", Empty, self._handle_abort)
rospy.Subscriber("imc/imc_heartbeat", Empty,
self._handle_imc_heartbeat)
rospy.Subscriber("imc/plan_db", PlanDB, self._handle_plan_db)
rospy.Timer(rospy.Duration(1, 0), self._send_estimated_state)
rospy.Timer(rospy.Duration(1, 0), self._send_plan_control_state)
def _send_waypoints(self, req):
# req = InitWaypointSetRequest()
res = InitWaypointSetResponse()
res.success = False
try:
for wp in req.waypoints:
if not wp.header.frame_id.startswith("world"):
p = PointStamped(
Header(0, rospy.Time.from_sec(0), wp.header.frame_id),
wp.point)
p = self._tf_buffer.transform(p, "world")
wp.point = p.point
wp.point.z = -abs(wp.point.z)
wp.header.frame_id = "world"
res = self._waypoint_setter(req)
return res
except Exception as e:
rospy.logerr("{} | {}".format(rospy.get_name(), e.message))
return res
def _handle_goto(self, msg):
"""Parse if a goto message is received"""
# TODO Not implemented in imc_ros_bridge yet
pass
def _parse_plan(self):
# TODO this parser only accepts a set of goto commands
"""A plan has been requested by neptus to start. Parse the plan here and send to controller."""
geopoints = []
speeds = []
for maneuver in self._current_plan.plan_spec.maneuvers:
geopoints.append(
GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi,
maneuver.maneuver.lon * 180.0 / np.pi,
maneuver.maneuver.z))
speeds.append(maneuver.maneuver.speed)
if self._datum is not None:
geopoints.append(self._datum)
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
points = self._geo_converter.call(req).utmpoints
wps = []
# If datum is available, then reference frame is ENU World
if self._datum is not None:
for point, speed in zip(points[:-1], speeds):
wp = Waypoint()
wp.point = Point(point.x - points[-1].x,
point.y - points[-1].y,
-point.z - points[-1].z)
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "world"
wp.radius_of_acceptance = 3.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
# If datum is not given, then reference frame is ENU UTM TODO: LOOKUP TRANSFORM
else:
for point, speed in zip(points, speeds):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = -point.z
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "utm"
wp.radius_of_acceptance = 3.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
self.STATE = PlanControlState.READY
req = InitWaypointSetRequest()
req.start_time = Time(rospy.Time.from_sec(0))
req.start_now = True
req.waypoints = wps
req.max_forward_speed = max(speeds)
req.interpolator.data = "lipb"
req.heading_offset = 0
self._waypoint_setter.wait_for_service()
if self._waypoint_setter(Time(rospy.Time.from_sec(0.0)), True, wps,
max(speeds), 0.0, String("lipb")):
self.STATE = PlanControlState.EXECUTING
else:
self.STATE = PlanControlState.FAILURE
def _handle_plan_control(self, msg):
"""Parse requests to start or stop a plan."""
typee = msg.type
op = msg.op
# request to start a mission!
if typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_SET:
self._current_plan = self._plan_db[msg.plan_id]
self.STATE = self._plan_control_state_msg.INITIALIZING
self._parse_plan()
# request to stop mission
elif typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_DEL:
self._current_plan = PlanDB()
self._hold_position.wait_for_service()
self._hold_position()
self.STATE = self._plan_control_state_msg.READY
def _handle_abort(self, msg):
"""Listen for the abort message, do something if it comes :)"""
pass
def _handle_imc_heartbeat(self, msg):
"""Here in case you want to do something on the heartbeat of the IMC"""
pass
def _handle_plan_db(self, msg):
"""Parse requests for Plan Database messages"""
typee = msg.type
op = msg.op
# request for plan info
if typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_GET_INFO:
# we need to respond to this with some info... but what?
rospy.loginfo_throttle_identical(
30, "Got REQUEST GET_INFO planDB msg from Neptus for plan: {}".
format(str(msg.plan_id)))
response = PlanDB()
response.plan_id = self._plan_db[msg.plan_id].plan_id
response.type = imc_enums.PLANDB_TYPE_SUCCESS
response.op = imc_enums.PLANDB_OP_GET_INFO
response.plandb_information = self._plan_db[
msg.plan_id].plandb_information
self._plan_db_pub.publish(response)
rospy.loginfo_throttle_identical(
30, "Answered GET_INFO for plan:" + str(response.plan_id))
# request for plan state
elif typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_GET_STATE:
rospy.loginfo_throttle_identical(
30, "Got REQUEST GET_STATE planDB msg from Neptus")
response = PlanDB()
response.plan_id = self._current_plan.plan_id
response.type = imc_enums.PLANDB_TYPE_SUCCESS
response.op = imc_enums.PLANDB_OP_GET_STATE
response.plandb_state = PlanDBState()
response.plandb_state.plan_count = len(self._plan_db)
response.plandb_state.plans_info = []
totalSize = 0
latest = None
# TODO Neptus needs an md5 calculation on the plans_info message to be stored in plandb_state.md5 in order
# to sync, but we cannot seem to get that right for now.
# https://github.com/LSTS/imcjava/blob/d95fddeab4c439e603cf5e30a32979ad7ace5fbc/src/java/pt/lsts/imc/adapter/PlanDbManager.java#L160
# See above for an example
# It seems like we need to keep a planDB ourselves on this side, collect all the plans we
# received and answer this get_state with data from them all to get a properly synchronized plan.
buffer = StringIO(
) # This buffer method for calculating md5 sum of a ros message is not ideal.
md = hashlib.md5()
for p in self._plan_db.values():
pdi = p.plandb_information
totalSize += pdi.plan_size
response.plandb_state.plans_info.append(pdi)
latest = latest if latest is None else max(
latest, pdi.change_time)
pdi.serialize(buffer)
[md.update(i) for i in buffer.buflist]
response.plandb_state.plan_count = len(
response.plandb_state.plans_info)
response.plandb_state.plan_size = totalSize
response.plandb_state.md5 = md.digest()
self._plan_db_pub.publish(response)
rospy.loginfo_throttle_identical(30,
"Answered GET_STATE for plans.")
# ack for plan set success
elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_SET:
rospy.loginfo_throttle_identical(
20, "Received SUCCESS for plandb set")
# ack for plan get info
elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_GET_INFO:
rospy.loginfo_throttle_identical(
20, "Received SUCCESS for plandb get info")
# ack for plan get state
elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_GET_STATE:
rospy.loginfo_throttle_identical(
20, "Received SUCCESS for plandb get state")
# request to set plan
elif op == imc_enums.PLANDB_OP_SET:
# update the plan_info and plan_state attributes
msg.plandb_information.plan_id = msg.plan_id
msg.plandb_information.plan_size = len(msg.plan_spec.maneuvers)
msg.plandb_information.change_time = time.time()
msg.plandb_information.md5 = msg.plan_spec_md5
self._plan_db.update({msg.plan_spec.plan_id: msg
}) # place the plan in a dictionary of plans
# send a success response
response = PlanDB()
response.type = imc_enums.PLANDB_TYPE_SUCCESS
response.op = imc_enums.PLANDB_OP_SET
response.plan_id = msg.plan_spec.plan_id
self._plan_db_pub.publish(response)
# handle other requests
else:
rospy.loginfo_throttle_identical(
5, "Received some unhandled planDB message:\n" + str(msg))
def _handle_gps(self, msg):
# neptus expects latitude and longitude to be in radians
self._estimated_state_msg.lat = msg.latitude * np.pi / 180.0
self._estimated_state_msg.lon = msg.longitude * np.pi / 180.0
self._estimated_state_msg.alt = msg.altitude
def _handle_pose(self, msg):
if self._tf_buffer.can_transform("bluerov2/base_link", "world_ned",
rospy.Time.now(),
rospy.Duration.from_sec(0.5)):
tf = self._tf_buffer.lookup_transform("bluerov2/base_link",
"world_ned",
rospy.Time.now(),
rospy.Duration.from_sec(0.5))
else:
return
# TODO neptus gets confused if you try to give all available estimates, there is probably a transformation problem
# self._estimated_state_msg.x = tf.transform.translation.x
# self._estimated_state_msg.y = tf.transform.translation.y
# self._estimated_state_msg.z = tf.transform.translation.z
# self._estimated_state_msg.depth = tf.transform.translation.z
# self._estimated_state_msg.height = msg.pose.pose.position.z
R = Rotation([
tf.transform.rotation.x, tf.transform.rotation.y,
tf.transform.rotation.z, tf.transform.rotation.w
])
self._estimated_state_msg.phi, self._estimated_state_msg.theta, self._estimated_state_msg.psi = R.as_euler(
"xyz", False).squeeze()
# self._estimated_state_msg.u = msg.twist.twist.linear.x
# self._estimated_state_msg.v = -msg.twist.twist.linear.y
# self._estimated_state_msg.w = -msg.twist.twist.linear.z
# self._estimated_state_msg.p = msg.twist.twist.angular.x
# self._estimated_state_msg.q = -msg.twist.twist.angular.y
# self._estimated_state_msg.r = -msg.twist.twist.angular.z
# u = np.array([msg.twist.twist.linear.x,msg.twist.twist.linear.y,msg.twist.twist.linear.z])
# self._estimated_state_msg.vx, self._estimated_state_msg.vy, self._estimated_state_msg.vz = R.apply(u, True)
def _send_estimated_state(self, event):
self._estimated_state_pub.publish(self._estimated_state_msg)
def _send_plan_control_state(self, event):
self._plan_control_state_msg.state = self.STATE
self._plan_control_state_pub.publish(self._plan_control_state_msg)
