def update(self): plan_control_msg = self.plan_control_msg if plan_control_msg is None: # not receiving anything is ok. return pt.Status.SUCCESS # check if this message is a 'go' or 'no go' message # imc/plan_control(569): # int type:[0,1,2,3] req,suc,fail,in prog # int op:[0,1,2,3] start, stop, load, get # int request_id # string plan_id # int flags # string info # the start button in neptus sends: # type:0 op:0 plan_id:"string" flags:1 # stop button sends: # type:0 op:1 plan_id:'' flags:1 # teleop button sends: # type:0 op:0 plan_id:"teleoperation-mode" flags:0 typee = plan_control_msg.type op = plan_control_msg.op plan_id = plan_control_msg.plan_id flags = plan_control_msg.flags # somehow this happens... if plan_id is None: plan_id='' # separate well-defined ifs for possible future shenanigans. if typee==0 and op==0 and plan_id!='' and flags==1: # start button # check if the start was given for our current plan current_mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) self.bb.set(bb_enums.PLAN_IS_GO, True) self.bb.set(bb_enums.ENABLE_AUTONOMY, False) if current_mission_plan is not None and plan_id == current_mission_plan.plan_id: rospy.loginfo("Started plan:{}".format(plan_id)) else: if current_mission_plan is None: rospy.logwarn("Start given for plan:{} but we don't have a plan!".format(plan_id)) else: rospy.logwarn("Start given for plan:{} our plan:{}".format(plan_id, current_mission_plan.plan_id)) if typee==0 and op==1 and plan_id=='' and flags==1: # stop button self.bb.set(bb_enums.PLAN_IS_GO, False) self.bb.set(bb_enums.ENABLE_AUTONOMY, False) # this string is hardcoded in Neptus, so we hardcode it here too! if typee==0 and op==0 and plan_id=='teleoperation-mode' and flags==0: # teleop button self.bb.set(bb_enums.ENABLE_AUTONOMY, True) rospy.logwarn_throttle_identical(10, "AUTONOMOUS MODE") # reset it until next message self.plan_control_msg = None return pt.Status.SUCCESS
def update(self): poi = self.bb.get(bb_enums.POI_POINT_STAMPED) if poi is None: rospy.loginfo_throttle_identical(10, "No POI :(") return pt.Status.SUCCESS if self._last_known_poi is None: # a poi exists but we didnt know any beforehand, new poi! self._last_known_poi = poi rospy.logwarn_throttle_identical(10, "Our first POI!") return pt.Status.FAILURE # we knew a poi, there is a poi we see, far enough? xdiff = poi.point.x - self._last_known_poi.point.x ydiff = poi.point.y - self._last_known_poi.point.y zdiff = poi.point.z - self._last_known_poi.point.z dist = math.sqrt(xdiff**2 + ydiff**2 + zdiff**2) # its far enough! if dist > self.new_poi_distance: self._last_known_poi = poi rospy.logwarn_throttle_identical( 10, "A new POI that is far enough!" + str(dist)) return pt.Status.FAILURE # aww, not far enough self._last_known_poi = poi rospy.loginfo_throttle_identical(10, "Probably the same POI as before...") return pt.Status.SUCCESS
def control_update(self, timer_event): failsafe = False if self.eta is None or self.v is None: rospy.logwarn_throttle_identical(3, 'Odometry not yet received: Controller waiting...') failsafe = True if self.last_updated is None or rospy.Time.now().to_sec() - self.last_updated > self.timeout_duration: failsafe = True rospy.logwarn_throttle_identical(3, 'No controller command received recently: entering failsafe mode!') if not failsafe: eta = self.eta v = self.v ta = self.target_acc ty = self.target_yaw_acc roll_error = self.target_roll - eta[3] pitch_error = self.target_pitch - eta[4] # Limit error to [-pi, pi] to ensure that the robot rotates the least amount # to hit the target roll/pitch: if roll_error > np.pi: roll_error -= 2*np.pi if roll_error < -np.pi: roll_error += 2 * np.pi if pitch_error > np.pi: pitch_error -= 2*np.pi if pitch_error < -np.pi: pitch_error += 2 * np.pi # Compute efforts from PID: roll_effort = self.roll_pid(-roll_error) pitch_effort = self.pitch_pid(-pitch_error) # Build the accel in world space: vd_pid = [0, 0, 0, roll_effort, pitch_effort, 0] # Convert worldspace accel to body frame: R = Rotation.from_euler('xyz', eta[3:6]).inv() target_acc_body = R.apply(ta) target_yaw_body = R.apply([0, 0, ty]) vd_command = np.hstack((target_acc_body, target_yaw_body)) vd = np.array(vd_pid) + np.array(vd_command) # Compute force required for target acceleration and current velocity: # (Inverse dynamics) tau = self.dyn.compute_tau(eta, v, vd) else: tau = [0] * 6 # Build and publish control wrench: wrench = WrenchStamped() wrench.header.stamp = rospy.Time.now() wrench.header.frame_id = self.base_link wrench.wrench.force = Vector3(tau[0], -tau[1], -tau[2]) wrench.wrench.torque = Vector3(tau[3], -tau[4], -tau[5]) self.pub_wrench.publish(wrench)
def update(self): # try not to call the service every tick... dvl_is_running = self.bb.get(bb_enums.DVL_IS_RUNNING) if dvl_is_running is not None: if dvl_is_running == self.sb.data: rospy.loginfo_throttle_identical(20, "DVL is already running:"+str(self.sb.data)) return pt.Status.SUCCESS # check if enough time has passed since last call t = time.time() if t - self.last_toggle < self.cooldown: # nope, return running while we wait rospy.loginfo_throttle_identical(5, "Waiting on DVL toggle cooldown") return pt.Status.RUNNING try: ret = self.switcher_service(self.running) except rospy.service.ServiceException: rospy.logwarn_throttle_identical(60, "DVL Start/stop service not found! Succeeding by default namespace:{}".format(self.service_name)) return pt.Status.SUCCESS if ret.success: rospy.loginfo_throttle_identical(5, "DVL TOGGLED:"+str(self.sb.data)) self.last_toggle = time.time() self.bb.set(bb_enums.DVL_IS_RUNNING, self.sb.data) return pt.Status.SUCCESS rospy.logwarn_throttle_identical(5, "DVL COULD NOT BE TOGGLED:{}, ret:{}".format(self.sb.data, ret)) return pt.Status.FAILURE
def update(self): world_trans = self.bb.get(bb_enums.WORLD_TRANS) # get the utm zone of our current lat,lon utmz = self.bb.get(bb_enums.UTM_ZONE) band = self.bb.get(bb_enums.UTM_BAND) if utmz is None or band is None or world_trans is None: reason = "Could not update current lat/lon!" rospy.logwarn_throttle_identical(5, reason) self.feedback_message = reason return pt.Status.FAILURE # get positional feedback of the p2p goal easting, northing = world_trans[0], world_trans[1] # make utm point pnt = UTMPoint(easting=easting, northing=northing, altitude=0, zone=utmz, band=band) # get lat-lon pnt = pnt.toMsg() self.bb.set(bb_enums.CURRENT_LATITUDE, pnt.latitude) self.bb.set(bb_enums.CURRENT_LONGITUDE, pnt.longitude) return pt.Status.SUCCESS
def update(self): lat = self.bb.get(bb_enums.CURRENT_LATITUDE) lon = self.bb.get(bb_enums.CURRENT_LONGITUDE) depth = self.bb.get(bb_enums.DEPTH) world_rot = self.bb.get(bb_enums.WORLD_ROT) if depth is None: reason = "depth was None, using 0" self.feedback_message = reason depth = 0 if lat is None or lon is None or world_rot is None: rospy.logwarn_throttle_identical( 10, "Could not update neptus estimated state because lat/lon/world_rot was None!" ) return pt.Status.SUCCESS # construct message for neptus e_state = EstimatedState() e_state.lat = np.radians(lat) e_state.lon = np.radians(lon) e_state.depth = depth roll, pitch, yaw = tf.transformations.euler_from_quaternion(world_rot) e_state.psi = np.pi / 2. - yaw # send the message to neptus self.estimated_state_pub.publish(e_state) return pt.Status.SUCCESS
def initialise(self): if not self.action_server_ok: rospy.logwarn_throttle_identical(5, "No Action Server found for emergency action, will just block the tree!") return rospy.logwarn("EMERGENCY SURFACING") # construct the message self.action_goal = GotoWaypointGoal() self.sent_goal = False
def respond_set_success(self): current_mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) if current_mission_plan is None: rospy.logwarn_throttle_identical(30, "No mission plan obj!") return plan_id = current_mission_plan.plan_id self.plandb_msg.plan_id = plan_id self.plandb_pub.publish(self.plandb_msg) rospy.loginfo_throttle_identical(30, "Answered set success for plan_id:"+str(plan_id))
def setup(self, timeout): if self.no_service(): return True self.path_pub = rospy.Publisher(self.path_topic, Path, queue_size=1) try: rospy.wait_for_service(self.path_planner_service_name, timeout) self.path_planner = rospy.ServiceProxy( self.path_planner_service_name, trajectory) self.service_ok = True except: rospy.logwarn_throttle_identical( 5, "Can not reach the path planner at:" + self.path_planner_service_name) return True
def read_plandb(plandb, plan_frame, local_frame): """ planddb message is a bunch of nested objects, we want a list of waypoints in the local frame, """ tf_listener = tf.TransformListener() try: tf_listener.waitForTransform(plan_frame, local_frame, rospy.Time(), rospy.Duration(4.0)) except: rospy.logerr_throttle(5, "Could not find tf from:"+plan_frame+" to:"+local_frame) waypoints = [] waypoint_man_ids = [] request_id = plandb.request_id plan_id = plandb.plan_id plan_spec = plandb.plan_spec for plan_man in plan_spec.maneuvers: man_id = plan_man.maneuver_id man_name = plan_man.maneuver.maneuver_name man_imc_id = plan_man.maneuver.maneuver_imc_id maneuver = plan_man.maneuver # probably every maneuver has lat lon z in them, but just in case... if man_imc_id == imc_enums.MANEUVER_GOTO: lat = maneuver.lat lon = maneuver.lon depth = maneuver.z utm_point = fromLatLong(np.degrees(lat), np.degrees(lon)).toPoint() stamped_utm_point = PointStamped() stamped_utm_point.header.frame_id = plan_frame stamped_utm_point.header.stamp = rospy.Time(0) stamped_utm_point.point.x = utm_point.x stamped_utm_point.point.y = utm_point.y stamped_utm_point.point.z = depth try: waypoint_local = tf_listener.transformPoint(local_frame, stamped_utm_point) # because the frame changes changes depth, we really want the original depth waypoint = (waypoint_local.point.x, waypoint_local.point.y, depth) waypoints.append(waypoint) waypoint_man_ids.append(man_id) except: rospy.logwarn_throttle_identical(10, "Can not transform plan point to local point!") else: rospy.logwarn("SKIPPING UNIMPLEMENTED MANEUVER:", man_imc_id, man_name) return waypoints, waypoint_man_ids
def update(self): if not self.action_server_ok: self.feedback_message = "Action Server for emergency action can not be used!" rospy.logerr_throttle_identical(5, self.feedback_message) return pt.Status.FAILURE # if your action client is not valid if not self.action_client: self.feedback_message = "ActionClient for emergency action is invalid!" rospy.logwarn_throttle_identical(5, self.feedback_message) return pt.Status.FAILURE # if the action_goal is invalid if not self.action_goal: self.feedback_message = "No action_goal!" rospy.logwarn(self.feedback_message) return pt.Status.FAILURE # if goal hasn't been sent yet if not self.sent_goal: self.action_goal_handle = self.action_client.send_goal( self.action_goal, feedback_cb=self.feedback_cb) self.sent_goal = True rospy.loginfo("Sent goal to action server:" + str(self.action_goal)) self.feedback_message = "Emergency goal sent" return pt.Status.RUNNING # if the goal was aborted or preempted if self.action_client.get_state() in [ actionlib_msgs.GoalStatus.ABORTED, actionlib_msgs.GoalStatus.PREEMPTED ]: self.feedback_message = "Aborted emergency" rospy.loginfo(self.feedback_message) return pt.Status.FAILURE result = self.action_client.get_result() # if the goal was accomplished if result: self.feedback_message = "Completed emergency" rospy.loginfo(self.feedback_message) return pt.Status.SUCCESS # if we're still trying to accomplish the goal return pt.Status.RUNNING
def update(self): data = self.bb.get(bb_enums.GPS_FIX) if (data is None or data.latitude is None or data.latitude == 0.0 or data.longitude is None or data.latitude == 0.0 or data.status.status == -1): rospy.loginfo_throttle_identical( self._spam_period, "GPS lat/lon are 0s or Nones, cant set utm zone/band from these >:( " ) # shitty gps self._spam_period = min(self._spam_period * 2, self._max_spam_period) return pt.Status.SUCCESS self.gps_zone, self.gps_band = fromLatLong(data.latitude, data.longitude).gridZone() if self.gps_zone is None or self.gps_band is None: rospy.logwarn_throttle_identical( 10, "gps zone and band from fromLatLong was None") return pt.Status.SUCCESS # first read the UTMs given by ros params prev_band = self.bb.get(bb_enums.UTM_BAND) prev_zone = self.bb.get(bb_enums.UTM_ZONE) if prev_zone != self.gps_zone or prev_band != self.gps_band: rospy.logwarn_once( "PREVIOUS UTM AND GPS_FIX UTM ARE DIFFERENT!\n Prev:" + str((prev_zone, prev_band)) + " gps:" + str((self.gps_zone, self.gps_band))) if common_globals.TRUST_GPS: rospy.logwarn_once("USING GPS UTM!") self.bb.set(bb_enums.UTM_ZONE, self.gps_zone) self.bb.set(bb_enums.UTM_BAND, self.gps_band) else: rospy.logwarn_once("USING PREVIOUS UTM!") self.bb.set(bb_enums.UTM_ZONE, prev_zone) self.bb.set(bb_enums.UTM_BAND, prev_band) return pt.Status.SUCCESS
def publish_thrust_forces(self, control_forces, control_torques, frame_id=None): if not self.ready: return max_thrust = self.config['max_thrust'] if frame_id is not None: if self.config['base_link'] != frame_id: assert self.base_link_ned_to_enu is not None, 'Transform from' ' base_link_ned to base_link could not be found' if 'base_link_ned' not in self.config['base_link']: control_forces = numpy.dot(self.base_link_ned_to_enu, control_forces) control_torques = numpy.dot(self.base_link_ned_to_enu, control_torques) else: control_forces = numpy.dot(self.base_link_ned_to_enu.T, control_forces) control_torques = numpy.dot(self.base_link_ned_to_enu.T, control_torques) gen_forces = numpy.hstack( (control_forces, [0, 0, 0])).transpose() thrust_for_forces = self.compute_thruster_forces(gen_forces, "forces") gen_torques = numpy.hstack( ([0, 0, 0], control_torques)).transpose() thrust_for_torques = self.compute_thruster_forces(gen_torques, "torques") thrust_requested = thrust_for_forces + thrust_for_torques max_req_thrust = max(thrust_requested) if max_req_thrust > max_thrust: rospy.logwarn_throttle_identical(1, '[Thruster Manager] Thrusters Saturated! Reducing forces to compensate.') max_lin_thrust = max_thrust - max(thrust_for_torques) scale_factor = max_lin_thrust / max(thrust_for_forces) thrust_for_forces = thrust_for_forces * scale_factor self.thrust = thrust_for_forces + thrust_for_torques if max(self.thrust) > max_thrust + 0.1: rospy.logwarn_throttle_identical(1, '[Thruster Manager] Critical error in thrust limiting!') self.command_thrusters()
def compute_thruster_forces(self, gen_forces, info_string="N/A"): """Compute desired thruster forces using the inverse configuration matrix. """ # Calculate individual thrust forces thrust = self.inverse_configuration_matrix.dot(gen_forces) # Obey limit on max thrust by applying a constant scaling factor to all # thrust forces limitation_factor = 1.0 # if type(self.config['max_thrust']) == list: # if len(self.config['max_thrust']) != self.n_thrusters: # raise rospy.ROSException('max_thrust list must have the length' # ' equal to the number of thrusters') # max_thrust = self.config['max_thrust'] # else: max_thrust = self.config['max_thrust'] highest_req_thrust = max(thrust) if highest_req_thrust > max_thrust: rospy.logwarn_throttle_identical(1, '[Thruster Manager] Thrusters Saturated! Source: {}!'.format(info_string)) scale_factor = max_thrust / highest_req_thrust thrust = thrust * scale_factor return thrust
def _pose_callback(self, pose_msg): if self._state_timestamp is None: self._state_timestamp = pose_msg.header.stamp return if not self._controller_enabled: rospy.logwarn_throttle_identical( 1, "Engine controller disabled, control loop disengaged") return # Validate input based on speed estimation. This is just a hack to # solve a transient in the EKF filter during startup. # TODO Figure out why there's a transient in the ekf filter during startup absolute_velocity = math.sqrt(pose_msg.twist.twist.linear.x**2 + pose_msg.twist.twist.linear.y**2) if (absolute_velocity > self._max_valid_odometry_velocity): # Ignore this sample rospy.logwarn_throttle_identical( 3, "Ignoring pose message, speed out of range") return # If we haven't received a command in a while, reset expected values now = rospy.Time.now() age_of_command = (now - self._latest_command_timestamp).to_sec() if (age_of_command > self._max_command_duration): self._latest_command_timestamp = now rospy.logwarn( "Ran too long with no commands, resetting loop setpoints to zero." ) self._reset_control_setpoints() delta_t = (pose_msg.header.stamp - self._state_timestamp).to_sec() self._state_timestamp = pose_msg.header.stamp rospy.loginfo_throttle_identical(1, "Loop time delta: %f" % (delta_t)) current_linear_speed = pose_msg.twist.twist.linear.x current_angular_speed = pose_msg.twist.twist.angular.z rospy.loginfo( "Loop inputs:\n linear setpoint=%f\n linear feedback=%f\n angular setpoint=%f\n angular feedback=%f" % (self._expected_linear_speed, current_linear_speed, self._expected_angular_speed, current_angular_speed)) linear_speed_error = self._expected_linear_speed - current_linear_speed angular_speed_error = self._expected_angular_speed - current_angular_speed linear_speed_control_action = self._linear_control.update_filter_state( linear_speed_error, delta_t) angular_speed_control_action = self._angular_control.update_filter_state( angular_speed_error, delta_t) within_actuator_range = self.send_engine_messages( linear_speed_control_action - angular_speed_control_action, linear_speed_control_action + angular_speed_control_action) if (not within_actuator_range): rospy.logwarn( "Actuators outside of control range, anti-windup activated!") self._linear_control.reset_state() self._angular_control.reset_state()
def test_log(self): rosout_logger = logging.getLogger('rosout') import rospy self.assertTrue(len(rosout_logger.handlers) == 2) self.assertTrue(rosout_logger.handlers[0], rosgraph.roslogging.RosStreamHandler) self.assertTrue(rosout_logger.handlers[1], rospy.impl.rosout.RosOutHandler) default_ros_handler = rosout_logger.handlers[0] # Remap stdout for testing lout = StringIO() lerr = StringIO() test_ros_handler = rosgraph.roslogging.RosStreamHandler(colorize=False, stdout=lout, stderr=lerr) try: # hack to replace the stream handler with a debug version rosout_logger.removeHandler(default_ros_handler) rosout_logger.addHandler(test_ros_handler) import rospy rospy.loginfo("test 1") lout_last = lout.getvalue().splitlines()[-1] self.assert_("test 1" in lout_last) rospy.logwarn("test 2") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[WARN]" in lerr_last) self.assert_("test 2" in lerr_last) rospy.logerr("test 3") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[ERROR]" in lerr_last) self.assert_("test 3" in lerr_last) rospy.logfatal("test 4") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[FATAL]" in lerr_last) self.assert_("test 4" in lerr_last) # logXXX_once lout_len = -1 for i in range(3): rospy.loginfo_once("test 1") lout_last = lout.getvalue().splitlines()[-1] if i == 0: self.assert_("test 1" in lout_last) lout_len = len(lout.getvalue()) else: # making sure the length of lout doesnt change self.assert_(lout_len == len(lout.getvalue())) lerr_len = -1 for i in range(3): rospy.logwarn_once("test 2") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 2" in lerr_last) lerr_len = len(lerr.getvalue()) else: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) lerr_len = -1 for i in range(3): rospy.logerr_once("test 3") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 3" in lerr_last) lerr_len = len(lerr.getvalue()) else: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) lerr_len = -1 for i in range(3): rospy.logfatal_once("test 4") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 4" in lerr_last) lerr_len = len(lerr.getvalue()) else: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) # logXXX_throttle lout_len = -1 for i in range(3): rospy.loginfo_throttle(3, "test 1") lout_last = lout.getvalue().splitlines()[-1] if i == 0: self.assert_("test 1" in lout_last) lout_len = len(lout.getvalue()) rospy.sleep(rospy.Duration(1)) elif i == 1: # making sure the length of lout doesnt change self.assert_(lout_len == len(lout.getvalue())) rospy.sleep(rospy.Duration(2)) else: self.assert_("test 1" in lout_last) lerr_len = -1 for i in range(3): rospy.logwarn_throttle(3, "test 2") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 2" in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(1)) elif i == 1: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) rospy.sleep(rospy.Duration(2)) else: self.assert_("test 2" in lerr_last) lerr_len = -1 for i in range(3): rospy.logerr_throttle(3, "test 3") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 3" in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(1)) elif i == 1: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) rospy.sleep(rospy.Duration(2)) else: self.assert_("test 3" in lerr_last) lerr_len = -1 for i in range(3): rospy.logfatal_throttle(3, "test 4") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 4" in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(1)) elif i == 1: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) rospy.sleep(rospy.Duration(2)) else: self.assert_("test 4" in lerr_last) # logXXX_throttle_identical lout_len = -1 for i in range(5): if i < 2: test_text = "test 1" else: test_text = "test 1a" rospy.loginfo_throttle_identical(2, test_text) lout_last = lout.getvalue().splitlines()[-1] if i == 0: # Confirm test text was logged self.assert_(test_text in lout_last) lout_len = len(lout.getvalue()) elif i == 1: # Confirm the length of lout hasn't changed self.assert_(lout_len == len(lout.getvalue())) elif i == 2: # Confirm the new message was logged correctly self.assert_(test_text in lout_last) lout_len = len(lout.getvalue()) rospy.sleep(rospy.Duration(2)) elif i == 3: # Confirm the length of lout has changed self.assert_(lout_len != len(lout.getvalue())) else: # Confirm new test text is in last log self.assert_(test_text in lout_last) lerr_len = -1 for i in range(5): if i < 2: test_text = "test 2" else: test_text = "test 2a" rospy.logwarn_throttle_identical(2, test_text) lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: # Confirm test text was logged self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) elif i == 1: # Confirm the length of lerr hasn't changed self.assert_(lerr_len == len(lerr.getvalue())) elif i == 2: # Confirm the new message was logged correctly self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(2)) elif i == 3: # Confirm the length of lerr has incremented self.assert_(lerr_len != len(lerr.getvalue())) else: # Confirm new test text is in last log self.assert_(test_text in lerr_last) lerr_len = -1 for i in range(5): if i < 2: test_text = "test 3" else: test_text = "test 3a" rospy.logerr_throttle_identical(2, test_text) lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: # Confirm test text was logged self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) elif i == 1: # Confirm the length of lerr hasn't changed self.assert_(lerr_len == len(lerr.getvalue())) elif i == 2: # Confirm the new message was logged correctly self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(2)) elif i == 3: # Confirm the length of lerr has incremented self.assert_(lerr_len != len(lerr.getvalue())) else: # Confirm new test text is in last log self.assert_(test_text in lerr_last) lerr_len = -1 for i in range(5): if i < 2: test_text = "test 4" else: test_text = "test 4a" rospy.logfatal_throttle_identical(2, test_text) lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: # Confirm test text was logged self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) elif i == 1: # Confirm the length of lerr hasn't changed self.assert_(lerr_len == len(lerr.getvalue())) elif i == 2: # Confirm the new message was logged correctly self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(2)) elif i == 3: # Confirm the length of lerr has incremented self.assert_(lerr_len != len(lerr.getvalue())) else: # Confirm new test text is in last log self.assert_(test_text in lerr_last) rospy.loginfo("test child logger 1", logger_name="log1") lout_last = lout.getvalue().splitlines()[-1] self.assert_("test child logger 1" in lout_last) rospy.logwarn("test child logger 2", logger_name="log2") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[WARN]" in lerr_last) self.assert_("test child logger 2" in lerr_last) rospy.logerr("test child logger 3", logger_name="log3") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[ERROR]" in lerr_last) self.assert_("test child logger 3" in lerr_last) rospy.logfatal("test child logger 4", logger_name="log4") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[FATAL]" in lerr_last) self.assert_("test child logger 4" in lerr_last) finally: # restoring default ros handler rosout_logger.removeHandler(test_ros_handler) lout.close() lerr.close() rosout_logger.addHandler(default_ros_handler)
def execute_cb(self, goal): rospy.loginfo_throttle(5, "Goal received") success = True rate = rospy.Rate(11.) # 10hz counter = 0 while not rospy.is_shutdown(): self.yaw_pid_enable.publish(True) self.depth_pid_enable.publish(True) # Preempted if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) success = False # Stop thrusters self.rpm.thruster_1_rpm = 0. self.rpm.thruster_2_rpm = 0. self.rpm_pub.publish(self.rpm) self.yaw_pid_enable.publish(False) self.depth_pid_enable.publish(False) self.vel_pid_enable.publish(False) print('leader_follower_action: stopped thrusters') self._as.set_preempted(self._result, "Preempted WP action") return # Compute and Publish setpoints if counter % 1 == 0: # distance check is done in the BT, we will add CBFs here later, which will include # that distance as a constraint anyways # if sqrt(rel_trans[0]**2 + rel_trans[1]**2 + rel_trans[2]**2) < self.min_dist: # break #Check transform between SAM1 (leader- goal) and SAM2 (follower -self), define a goal point and call the go_to_point() function self.leader_frame = goal.target_pose.header.frame_id try: (follower_trans, follower_rot) = self.listener.lookupTransform( self.follower_odom, self.follower_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException): rospy.logwarn_throttle_identical( 5, "Could not get transform between " + self.leader_frame + " and " + self.follower_frame) success = False break except: rospy.logwarn_throttle_identical( 5, "Could not do tf lookup for some other reason") success = False break try: (leader_trans, leader_rot) = self.listener.lookupTransform( self.follower_odom, self.leader_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException): rospy.logwarn_throttle_identical( 5, "Could not get transform between " + self.leader_frame + " and " + self.follower_frame) success = False break except: rospy.logwarn_throttle_identical( 5, "Could not do tf lookup for some other reason") success = False break xdiff = leader_trans[0] - follower_trans[0] ydiff = leader_trans[1] - follower_trans[1] zdiff = leader_trans[2] - follower_trans[2] yaw_setpoint = math.atan2(ydiff, xdiff) depth_setpoint = -zdiff rospy.loginfo_throttle( 10, 'yaw_setpoint:' + str(yaw_setpoint) + ' depth_setpoint:' + str(depth_setpoint)) self.depth_pub.publish(depth_setpoint) if self.vel_ctrl_flag: rospy.loginfo_throttle_identical( 5, "vel ctrl, no turbo turn") #with Velocity control self.yaw_pid_enable.publish(True) self.yaw_pub.publish(yaw_setpoint) # Publish to velocity controller self.vel_pid_enable.publish(True) self.vel_pub.publish(self.vel_setpoint) self.roll_pub.publish(self.roll_setpoint) #rospy.loginfo("Velocity published") else: #turbo turn not included, no velocity control rospy.loginfo_throttle_identical( 5, "Normal WP following, no turbo turn") self.yaw_pid_enable.publish(True) self.yaw_pub.publish(yaw_setpoint) # Thruster forward self.rpm.thruster_1_rpm = self.forward_rpm self.rpm.thruster_2_rpm = self.forward_rpm self.rpm_pub.publish(self.rpm) #rospy.loginfo("Thrusters forward") counter += 1 rate.sleep() # Stop thruster self.vel_pid_enable.publish(False) self.rpm.thruster_1_rpm = 0.0 self.rpm.thruster_2_rpm = 0.0 self.rpm_pub.publish(self.rpm) #Stop controllers self.yaw_pid_enable.publish(False) self.depth_pid_enable.publish(False) self.vel_pid_enable.publish(False) if self._result: rospy.loginfo('%s: Succeeded' % self._action_name) else: rospy.logwarn_throttle_identical(3, '%s: Failed' % self._action_name) self._as.set_succeeded(self._result)