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)