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)