class OfflineTfBuffer():
def __init__(self, transforms):
self.t_front = transforms[0].header.stamp
self.t_back = transforms[-1].header.stamp
self.buff = Buffer(cache_time=(self.t_back - self.t_front),
debug=False)
for transform in transforms:
self.buff.set_transform(transform, '/auth')
def LookupTransform(self, target_frame, source_frame, times):
return [
self.buff.lookup_transform(target_frame, source_frame, t)
for t in times
]
def AvailableTimeRange(self, target_frame, source_frame, times):
if times[0] > self.t_back or times[-1] < self.t_front:
return []
idx_front = 0
idx_back = len(times) - 1
while times[idx_front] < self.t_front:
idx_front += 1
while times[idx_back] > self.t_back:
idx_back -= 1
return (idx_front, idx_back + 1)
def CanTransform(self, target_frame, source_frame, times):
return self.buff.can_transform(
target_frame, source_frame, times[0]) and self.buff.can_transform(
target_frame, source_frame, times[-1])
class ImcInterface(object):
def __init__(self):
self._estimated_state_pub = rospy.Publisher("imc/estimated_state",
EstimatedState,
queue_size=5)
self._estimated_state_msg = EstimatedState()
self._geo_converter = rospy.ServiceProxy("convert_points",
ConvertGeoPoints)
self._plan_db = dict()
self._current_plan = PlanDB()
self._tf_buffer = Buffer()
TransformListener(self._tf_buffer)
rospy.Subscriber("gps", NavSatFix, self._handle_gps)
rospy.Subscriber("odometry", 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)
self._waypoint_serv = rospy.Service("load_waypoints", InitWaypointSet,
self._send_goal)
self._action_client = actionlib.SimpleActionClient(
"follow_waypoints", FollowWaypointsAction)
self._mode_client = rospy.ServiceProxy("controller/set_control_mode",
SetControlMode)
self._plan_db_pub = rospy.Publisher("imc/plan_db",
PlanDB,
queue_size=10)
self._marker_pub = rospy.Publisher("current_waypoints",
WaypointSet,
queue_size=1)
def _send_goal(self, req):
res = InitWaypointSetResponse()
res.success = False
try:
self._mode_client.wait_for_service(rospy.Duration(5))
mode_req = SetControlModeRequest()
mode_req.mode.mode = mode_req.mode.LOSGUIDANCE
self._mode_client(mode_req)
self._action_client.wait_for_server(rospy.Duration(5))
goal = FollowWaypointsGoal()
goal.waypoints.header = Header(0, rospy.Time.now(), "utm")
goal.waypoints.start_time = Time(goal.waypoints.header.stamp)
goal.waypoints.waypoints = req.waypoints
self._marker_pub.publish(goal.waypoints)
self._action_client.send_goal(goal, self._handle_done,
self._handle_active,
self._handle_feedback)
res.success = True
return res
except Exception as e:
rospy.logerr("{} | {}".format(rospy.get_name(), e.message))
return res
def _handle_done(self, status, result):
rospy.loginfo(
"{} | Action completed, status: {}\n Waypoints completed: {}".
format(rospy.get_name(), status, result))
def _handle_active(self):
rospy.loginfo("{} | Waypoints set, Front Seat Driver active.".format(
rospy.get_name()))
def _handle_feedback(self, feedback):
rospy.loginfo_throttle(10, "{} | {}".format(rospy.get_name(),
feedback))
def _handle_goto(self, msg):
"""Parse if a goto message is received"""
# TODO Not implemented in imc_ros_bridge yet
rospy.loginfo(msg)
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 = []
is_depths = []
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)
is_depths.append(maneuver.maneuver.z_units == 1)
req = ConvertGeoPointsRequest()
req.geopoints = geopoints
points = self._geo_converter.call(req).utmpoints
wps = []
# If datum is available, then reference frame is ENU World
for point, speed, is_depth in zip(points, speeds, is_depths):
wp = Waypoint()
wp.point.x = point.x
wp.point.y = point.y
wp.point.z = -abs(point.z) if is_depth else abs(point.z)
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = "utm"
wp.radius_of_acceptance = 1.0
wp.max_forward_speed = speed
wp.use_fixed_heading = False
wp.heading_offset = 0.0
wps.append(wp)
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._send_goal(req)
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._parse_plan()
req = SetControlModeRequest()
req.mode.mode = req.mode.LOSGUIDANCE
self._mode_client(req)
# request to stop mission
elif typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_DEL:
self._current_plan = PlanDB()
req = SetControlModeRequest()
req.mode.mode = req.mode.HOLDPOSITION
self._mode_client(req)
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_abort(self, msg):
"""Listen for the abort message, do something if it comes :)"""
self._action_client.wait_for_server()
if self._action_client.get_state() == GoalStatus.ACTIVE:
self._action_client.cancel_goal()
self._mode_client.wait_for_service()
req = SetControlModeRequest()
req.mode.mode = req.mode.ABORT
self._mode_client(req)
def _handle_imc_heartbeat(self, msg):
"""Here in case you want to do something on the heartbeat of the IMC"""
pass
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", "map_ned",
rospy.Time.now(),
rospy.Duration.from_sec(0.5)):
tf = self._tf_buffer.lookup_transform("bluerov2/base_link",
"map_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)
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 TF_Publisher:
# Constructor
def __init__(self):
# Read Parameters From YAML File
self.turtlebot_odom_topic = get_param('turtlebot_odom_topic')
self.turtlebot_base_footprint_frame = get_param(
'turtlebot_base_footprint_frame')
self.map_frame = get_param('map_frame')
self.drone_odom_frame = get_param('drone_odom_frame')
self.join_tree_srv_name = get_param('join_tree_srv')
self.debug = get_param('debug')
# TF Broadcaster
self.tf_broadcast = TransformBroadcaster()
# TF Listener
self.tf_listen = Buffer()
self.tf_listener = TransformListener(self.tf_listen)
# Subscribers
Subscriber(self.turtlebot_odom_topic, Odometry, self.turtlebot_odom_cb)
# Services for Joining the Drone TF Tree to Main Tree (Localization)
self.join_tree_service = Service(self.join_tree_srv_name, JointTrees,
self.join_tree_srv_function)
# Parameters
self.drone_tf = TransformStamped()
self.turtlebot_tf = TransformStamped()
self.turtlebot_tf.header.frame_id = self.map_frame
self.turtlebot_tf.child_frame_id = self.turtlebot_base_footprint_frame
self.drone_tf.header.frame_id = self.map_frame
self.drone_tf.child_frame_id = self.drone_odom_frame
self.drone_tf.transform.rotation.w = 1.0
self.turtlebot_tf.transform.rotation.w = 1.0
# Flags
self.join_trees = False # Set True When The Two TF Trees are Joint in One
# Callback Function to Get Data from Turtlebot Odometry Topic
def turtlebot_odom_cb(self, odometry):
self.turtlebot_tf.transform.translation.x = odometry.pose.pose.position.x
self.turtlebot_tf.transform.translation.y = odometry.pose.pose.position.y
self.turtlebot_tf.transform.translation.z = odometry.pose.pose.position.z
self.turtlebot_tf.transform.rotation.x = odometry.pose.pose.orientation.x
self.turtlebot_tf.transform.rotation.y = odometry.pose.pose.orientation.y
self.turtlebot_tf.transform.rotation.z = odometry.pose.pose.orientation.z
self.turtlebot_tf.transform.rotation.w = odometry.pose.pose.orientation.w
# Service Handle Function to Join Drone to Main Tree after Localization
def join_tree_srv_function(self, command):
if self.debug:
print('\x1b[33;1m' + str('Joining the Two Separate Frame Tree!') +
'\x1b[0m')
# Set Flag to True
self.join_trees = command.status
# Check if we can calculate the TF Between Drones Bottom Camera and Ar Tag
while not self.tf_listen.can_transform(
self.drone_odom_frame, self.map_frame, Time(0), Duration(1.0)):
pass
# Return From Service Once Tree is Joint
return True
class Marker_Follower:
# Constructor
def __init__(self):
# TF Listener
self.tf = Buffer()
self.tf_listener = TransformListener(self.tf)
# Flags
self.initialize = False
self.marker_detected = False # True if Drone Camera has detected a Tag
self.recovery_behavior = False # Used in recovery() Function if Marker is Lost
# PID Parameters
self.last_t = 0.0
self.lin_i_x = 0.0
self.lin_i_y = 0.0
self.lin_i_z = 0.0
self.rot_i_z = 0.0
# Gains ( with Initial Values)
self.kp = 0.3
self.ki = 0.01
self.kd = 0.06
self.r_kp = 3.0
self.r_ki = 1.5
self.r_kd = 0.57
self.r_kp_s = 0.6
self.r_ki_s = 0.13
self.r_kd_s = 0.07
self.error = 0.25
self.max_speed = 1.0
self.max_z_speed = 1.5
# Parameter from YAML File
self.drone_base_link = get_param('drone_base_link')
self.drone_base_footprint = get_param('drone_base_footprint')
self.map_frame = get_param('map_frame')
self.ar_pose_frame = get_param('ar_pose_frame')
self.drone_camera_frame = get_param('drone_camera_frame')
self.drone_speed_topic = get_param('drone_speed_topic')
self.ar_pose_topic = get_param('ar_pose_topic')
self.time_to_considered_marker_lost = get_param(
'time_to_considered_marker_lost')
# Parameters
self.last_time_marker_seen = 0.0 # Hold the time that Tag last seen, for Setting Marker Detected flag ON/OFF
# Ar Marker Topic Subscriber
self.marker_subscriber = Subscriber('/ar_pose_marker', ARMarker,
self.marker_cb)
# Timer for Checking if Marker is Lost
self.check_marker_timer = Timer(Duration(0.05),
self.check_marker_existence)
# PID Function
def pose_pid(self, x, x_last, y, y_last, z, z_last, yaw, yaw_last):
# Initialize
kp_1 = ki_1 = kd_1 = 0.0 # For X-Control
kp_2 = ki_2 = kd_2 = 0.0 # For Y-Control
kp_3 = ki_3 = kd_3 = 0.0 # For Z-Control
kp_4 = ki_4 = kd_4 = 0.0 # For Yaw-Control
# Get current dt
dt = (time() - self.last_t)
self.last_t = time()
# Prevent High Fluctuations of Integral Term
if dt > 0.1:
self.lin_i_x = self.lin_i_y = self.lin_i_z = self.rot_i_z = 0.0
# Integral Term
self.lin_i_x += x * dt
self.lin_i_y += y * dt
self.lin_i_z += z * dt
self.rot_i_z += yaw * dt
# Derivative Term
lin_d_x = (x - x_last) / dt
lin_d_y = (y - y_last) / dt
lin_d_z = (z - z_last) / dt
rot_d_z = (yaw - yaw_last) / dt
# Choose Gain
# For X-Y : 0.3,0.01,0.06 ||| For Z: 0.3,0.01,0.06 ||| For Yaw: 3,1.5,0.57
if fabs(x) > 0.1 or fabs(y) > 0.1:
if fabs(x) > 0.1:
kp_1 = 0.3
ki_1 = 0.01
kd_1 = 0.06
if fabs(y) > 0.1:
kp_2 = 0.3
ki_2 = 0.01
kd_2 = 0.06
if fabs(np.rad2deg(yaw)) > 1.0:
# Rotate Full Speed
if fabs(x) < 0.2 and fabs(y) < 0.2:
kp_4 = 3
ki_4 = 1.5
kd_4 = 0.57
# Rotate Carefully!
else:
kp_4 = 0.6
ki_4 = 0.13
kd_4 = 0.07
if fabs(np.rad2deg(yaw)) < 1.0 and fabs(x) < 0.1 and fabs(y) < 0.1:
kp_3 = 0.3
ki_3 = 0.01
kd_3 = 0.06
velocity = Twist()
# Calculate Speed
velocity.linear.x = kp_1 * x + ki_1 * self.lin_i_x + kd_1 * lin_d_x
velocity.linear.y = kp_2 * y + ki_2 * self.lin_i_y + kd_2 * lin_d_y
velocity.linear.z = kp_3 * z + ki_3 * self.lin_i_z + kd_3 * lin_d_z
velocity.angular.z = -(kp_4 * yaw + ki_4 * self.rot_i_z +
kd_4 * rot_d_z)
# Fixing Limits in Speed Assigned to [-MAX,MAX]
velocity.linear.x = np.clip(velocity.linear.x, -self.max_speed,
self.max_speed)
velocity.linear.y = np.clip(velocity.linear.y, -self.max_speed,
self.max_speed)
velocity.linear.z = np.clip(velocity.linear.z, -self.max_z_speed,
self.max_z_speed)
velocity.angular.z = np.clip(velocity.angular.z, -self.max_speed,
self.max_speed)
return velocity
# Initialize PID Timer Function
def initialize_pid(self):
self.last_t = time()
self.lin_i_x = 0.0
self.lin_i_y = 0.0
self.lin_i_z = 0.0
self.rot_i_z = 0.0
# Initialize Gains Function
def initialize_gains(self,
kp=0.3,
ki=0.01,
kd=0.06,
r_kp=3.0,
r_ki=1.5,
r_kd=0.57,
r_kp_s=0.6,
r_ki_s=0.13,
r_kd_s=0.07,
error=0.25,
speed=1.0,
z_speed=1.5):
self.kp = kp
self.ki = ki
self.kd = kd
self.r_kp = r_kp
self.r_ki = r_ki
self.r_kd = r_kd
self.r_kp_s = r_kp_s
self.r_ki_s = r_ki_s
self.r_kd_s = r_kd_s
self.error = error
self.max_speed = speed
self.max_z_speed = z_speed
self.initialize = True
# Calculate Pose Error From Tag Function that Returns (x,y,z,yaw,height)
def calculate_error_from_tag(self):
# Calculate_TF and get Pose Error
if self.can_transform() and self.marker_detected:
# Get the most recent TF's
tf_ = self.tf.lookup_transform(self.ar_pose_frame,
self.drone_camera_frame, Time(0),
Duration(1.0))
h_tf = self.tf.lookup_transform(self.drone_base_footprint,
self.drone_base_link, Time(0),
Duration(1.0))
# The Height that Drone is currently fly
height = h_tf.transform.translation.z
# Calculate Yaw from TF
(_, _, yaw) = euler_from_quaternion([
tf_.transform.rotation.x, tf_.transform.rotation.y,
tf_.transform.rotation.z, tf_.transform.rotation.w
])
yaw += pi
# Manipulate Yaw to Rotate Drone in the Right Direction
if np.rad2deg(yaw) > 180:
yaw -= 2 * pi
# Get the Rotation Matrix
(r1, r2, r3, _) = quaternion_matrix([
tf_.transform.rotation.x, tf_.transform.rotation.y,
tf_.transform.rotation.z, tf_.transform.rotation.w
])
rotation_mat = np.asmatrix([r1[0:3], r2[0:3], r3[0:3]])
# Get Position in Matrix Form
pose_mat = np.asmatrix([[tf_.transform.translation.x],
[tf_.transform.translation.y],
[tf_.transform.translation.z]])
# Calculate the Cartesian Error
error = rotation_mat * pose_mat
return [error.item(0), error.item(1), error.item(2), yaw, height]
else:
return [-1, -1, -1, -1, -1]
# Callback Function for Ar Marker Topic that Saves Image and TF at the Right Time
def marker_cb(self, marker):
self.last_time_marker_seen = get_time()
# Function that Check if Marker is Detected Called from Timer
def check_marker_existence(self, event):
if get_time(
) < self.last_time_marker_seen + self.time_to_considered_marker_lost:
self.marker_detected = True
else:
self.marker_detected = False
# Check for TF Between Drones Camera and Tag Availability Function
def can_transform(self):
if self.tf.can_transform(self.ar_pose_frame, self.drone_camera_frame,
Time(0), Duration(1.0)):
return True
else:
return False
# Check if Target is Reached Function
def check_target_reached(self):
error = self.calculate_pose_error()
if error != [-1, -1, -1, -1, -1]:
if error[0] < 0.075 and error[1] < 0.05 and error[
4] < 1.7 and np.rad2deg(error[3]) < 0.08:
return True
else:
return False
else:
return False
# Drone Marker Retrieval Function
def drone_marker_retrieval(self):
# Create Temporary Speeds
speed = Twist()
# Switching Between Rotate and Going Up using Recovery Behavior Flag
if self.recovery_behavior:
self.recovery_behavior = False
speed.angular.z = 0.5
if not self.recovery_behavior:
self.recovery_behavior = True
speed.linear.z = 0.5
return speed
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)
