def update(self): mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) if mission_plan is None or mission_plan.is_complete(): return pt.Status.FAILURE if self.no_service() or not self.service_ok: # there is no path planner, just copy the coarse points to the refined side mission_plan.set_refined_waypoints(mission_plan.waypoints) return pt.Status.SUCCESS if len(mission_plan.waypoints) <= 1: # there is literally just one point, cant plan for that apparently mission_plan.set_refined_waypoints(mission_plan.waypoints) return pt.Status.SUCCESS # give the location of the auv as the first point in the plan # to prevent overshooting the first real waypoint ps = self.bb.get(bb_enums.LOCATION_POINT_STAMPED) trajectory_response = self.path_planner( mission_plan.get_pose_array(ps)) refined_path = trajectory_response.fine refined_path.header.frame_id = 'map' self.path_pub.publish(refined_path) mission_plan.set_refined_waypoints( mission_plan.path_to_list(refined_path)) rospy.loginfo_throttle_identical( 10, "Refined waypoints length:" + str(len(mission_plan.refined_waypoints))) return pt.Status.SUCCESS
def handle_request_get_state(self, plandb_msg): rospy.loginfo_throttle_identical( 30, "Got REQUEST GET_STATE planDB msg from Neptus") current_mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) if current_mission_plan is None: return # https://github.com/LSTS/imcjava/blob/d95fddeab4c439e603cf5e30a32979ad7ace5fbc/src/java/pt/lsts/imc/adapter/PlanDbManager.java#L160 # See above for an example # TODO 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. # lets try telling neptus that we just got one plan, maybe that'll be okay? # seems alright, but after this message is sent, the plan goes red :/ response = PlanDB() response.plan_id = current_mission_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 = 1 response.plandb_state.plans_info.append(self.make_plandb_info()) self.plandb_pub.publish(response) rospy.loginfo_throttle_identical( 30, "Answered GET_STATE for plan:\n" + str(response.plan_id))
def _timer_callback(self, event): if not self._enable_path_tracer: rospy.loginfo_throttle_identical(1, "Path execution is disabled") return if self._utm_path is None: rospy.logerr("Wayfinding is not possible, the path is empty!") return if self._currently_executing_plan and not self._move_base_observer.move_base_is_active( ): rospy.logwarn("Waypoint reached!") self._currently_executing_plan = False # Notice that this crapy-ass algorithm depend on the timer callback being slow enough # so that we can see an update status on the move base action server before the next # time the callback gets called, or we might trigger the goal more than once before # it actually gets executed if not self._currently_executing_plan: # Keep executing the plan in a loop self._utm_path_index += 1 if self._utm_path_index >= len(self._utm_path): self._utm_path_index = 0 self._plan_path() self._currently_executing_plan = True rospy.logwarn("Setting sails to new goal!: {}".format( self._utm_path[self._utm_path_index])) self._move_base_simple_goal_pub.publish( self._utm_path[self._utm_path_index])
def angle_wrap(self, angle): if (abs(angle) > 3.141516): angle = angle - ( abs(angle) / angle) * 2 * 3.141516 #Angle wrapping between -pi and pi rospy.loginfo_throttle_identical(20, "Angle Error Wrapped") return angle
def update(self): plan_is_go = self.bb.get(bb_enums.PLAN_IS_GO) self.feedback_message = "Plan is go:{}".format(plan_is_go) if plan_is_go is None or plan_is_go == False: rospy.loginfo_throttle_identical(5, "Waiting for start plan") return pt.Status.FAILURE 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 update(self): mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) if mission_plan is None or not mission_plan.is_complete(): rospy.loginfo_throttle_identical(5, "Plan is not done") return pt.Status.FAILURE self.feedback_message = "Current plan:{}".format(mission_plan.plan_id) return pt.Status.SUCCESS
def _twist_callback(self, twist_msg): self._expected_linear_speed = twist_msg.linear.x self._expected_angular_speed = twist_msg.angular.z self._latest_command_timestamp = rospy.Time.now() rospy.loginfo_throttle_identical( 1, "Linear speed setpoint: %f" % (self._expected_linear_speed)) rospy.loginfo_throttle_identical( 1, "Angular speed setpoint: %f" % (self._expected_angular_speed))
def handle_set_plan(self, plandb_msg): # there is a plan we can at least look at mission_plan = MissionPlan(plan_frame=self.utm_link, local_frame=self.local_link, plandb_msg=plandb_msg) self.bb.set(bb_enums.MISSION_PLAN_OBJ, mission_plan) self.bb.set(bb_enums.ENABLE_AUTONOMY, False) rospy.loginfo_throttle_identical( 5, "Set the mission plan to:" + str(mission_plan.waypoints))
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 _black_box_pose_callback(self, point_msg): rospy.loginfo_throttle_identical( 3, "Receiving infomation from the acoustic sensor...") pose = PoseStamped() pose.header = point_msg.header pose.pose.position = point_msg.point pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 1.0 self._black_box_pose = pose
def handle_set_plan(self, plandb_msg): # there is a plan we can at least look at mission_plan = MissionPlan(plan_frame = self.utm_link, plandb_msg = plandb_msg, latlontoutm_service_name = self.latlontoutm_service_name) self.bb.set(bb_enums.MISSION_PLAN_OBJ, mission_plan) self.bb.set(bb_enums.ENABLE_AUTONOMY, False) self.bb.set(bb_enums.MISSION_FINALIZED, False) rospy.loginfo_throttle_identical(5, "Set the mission plan to:{} and un-finalized the mission.".format(mission_plan))
def handle_plandb_msg(self): plandb_msg = self.latest_plandb_msg if plandb_msg is None: return typee = plandb_msg.type op = plandb_msg.op # request get_info if typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_GET_INFO: self.handle_request_get_info(plandb_msg) elif typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_GET_STATE: self.handle_request_get_state(plandb_msg) elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_SET: rospy.loginfo_throttle_identical( 20, "Received SUCCESS for plandb set") 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") 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") elif op == imc_enums.PLANDB_OP_SET: self.handle_set_plan(plandb_msg) else: rospy.loginfo_throttle_identical( 5, "Received some unhandled planDB message:\n" + str(plandb_msg))
def update(self): current_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) if current_plan is None: # there is no plan, it can not change self.last_known_id = None self.last_known_time = 0 rospy.loginfo_throttle_identical( 10, "There was no plan, plan is not changed") return pt.Status.SUCCESS if self.last_known_id is None: # this is the first plan we saw # record it, and let the tree tick again self.last_known_id = current_plan.plan_id self.last_known_time = current_plan.creation_time rospy.loginfo_throttle_identical( 10, "First time seeing any plan, plan is changed") return pt.Status.FAILURE if self.last_known_id == current_plan.plan_id and self.last_known_time < current_plan.creation_time: # this is the same plan, but it was sent again, this means a restart # so the plan IS changed rospy.loginfo_throttle_identical( 10, "Same plan_id, but the current plan is newer, plan is changed") self.last_known_id = current_plan.plan_id self.last_known_time = current_plan.creation_time return pt.Status.FAILURE rospy.loginfo_throttle_identical(60, "Plan is not changed") return pt.Status.SUCCESS
def update(self): # UNTESTED STUFF HERE, RETURN FAILURE TO KEEP PPL # FROM USING THIS ACTION return pt.Status.FAILURE if not self.poi_link_available: return pt.Status.FAILURE poi = self.bb.get(bb_enums.POI_POINT_STAMPED) if poi is None: return pt.Status.SUCCESS poi_local = self.tf_listener.transformPoint(self.utm_link, poi) x = poi_local.point.x y = poi_local.point.y depth = poi.point.z # construct the waypoints that we want to go to inspection_depth = max(1, depth - 5) radius = 10 # go east,west,north,south,center # so we do bunch of fly-overs waypoints = [ (x+radius, y, inspection_depth), (x-radius, y, inspection_depth), (x, y+radius, inspection_depth), (x, y-radius, inspection_depth), (x, y, 0) ] waypoint_man_ids = ['east', 'west', 'north', 'south', 'surface_center'] # construct a planDB message to be given to the mission_plan # we will not fill the plan_spec of this plandb message, # and instead call a different constructor of MissionPlan # to bypass the lat/lon stuff pdb = PlanDB() pdb.request_id = 42 pdb.plan_id = "POI" # set it in the tree mission_plan = MissionPlan(plan_frame = self.utm_link, plandb_msg = pdb, waypoints = waypoints, waypoint_man_ids=waypoint_man_ids, latlontoutm_service_name = self.latlontoutm_service_name) self.bb.set(bb_enums.MISSION_PLAN_OBJ, mission_plan) rospy.loginfo_throttle_identical(5, "Due to POI, set the mission plan to:"+str(mission_plan)) return pt.Status.SUCCESS
def handle_request_get_info(self, plandb_msg): # we need to respond to this with some info... but what? rospy.loginfo_throttle_identical(30, "Got REQUEST GET_INFO planDB msg from Neptus") current_mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) if current_mission_plan is None: return response = PlanDB() response.plan_id = current_mission_plan.plan_id response.type = imc_enums.PLANDB_TYPE_SUCCESS response.op = imc_enums.PLANDB_OP_GET_INFO response.plandb_information = self.make_plandb_info() self.plandb_pub.publish(response) rospy.loginfo_throttle_identical(30, "Answered GET_INFO for plan:"+str(response.plan_id))
def timer_callback(self, event): if self.nav_goal is None: #rospy.loginfo_throttle(30, "Nav goal is None!") return try: (trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return # TODO: we could use this code for the other check also goal_point = PointStamped() goal_point.header.frame_id = self.nav_goal_frame goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z #print("Checking if nav goal is reached!") start_pos = np.array(trans) end_pos = np.array([ self.nav_goal.position.x, self.nav_goal.position.y, self.nav_goal.position.z ]) # We check for success out of the main control loop in case the main control loop is # running at 300Hz or sth. like that. We dont need to check succes that frequently. xydiff = start_pos[:2] - end_pos[:2] zdiff = np.abs(np.abs(start_pos[2]) - np.abs(end_pos[2])) xydiff_norm = np.linalg.norm(xydiff) # rospy.logdebug("diff xy:"+ str(xydiff_norm)+' z:' + str(zdiff)) rospy.loginfo_throttle_identical(5, "Using Crosstrack Error") rospy.loginfo_throttle_identical( 5, "diff xy:" + str(xydiff_norm) + ' z:' + str(zdiff) + " WP tol:" + str(self.wp_tolerance) + "Depth tol:" + str(self.depth_tolerance)) if xydiff_norm < self.wp_tolerance and zdiff < self.depth_tolerance: rospy.loginfo("Reached WP!") self.x_prev = self.nav_goal.position.x self.y_prev = self.nav_goal.position.y self.nav_goal = None self._result.reached_waypoint = True
def update(self): mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) if mission_plan is None: msg = "No plan received yet" self.feedback_message = msg rospy.loginfo_throttle(5, msg) return pt.Status.FAILURE elif not mission_plan.is_complete(): msg = "Progress:{}/{} on plan {}".format( mission_plan.current_wp_index, len(mission_plan.waypoints), mission_plan.plan_id) self.feedback_message = msg rospy.loginfo_throttle_identical(5, msg) return pt.Status.FAILURE rospy.loginfo_throttle_identical(2, "Plan is complete!") self.feedback_message = "Current plan:{}".format(mission_plan.plan_id) return pt.Status.SUCCESS
def _task_callback(self, task_msg): task_name = task_msg.name task_state = task_msg.state task_ready_time = task_msg.ready_time task_running_time = task_msg.running_time task_elapsed_time = task_msg.elapsed_time task_current_score = task_msg.score if task_state != self._current_task_state: self._current_task_state = task_state self._transition_to_state(task_name, task_state) rospy.loginfo("Task message received:") rospy.loginfo(" - Task name : {}".format(task_name)) rospy.loginfo(" - Task state : {}".format(task_state)) rospy.loginfo(" - Ready time : {}".format(task_ready_time)) rospy.loginfo(" - Running time : {}".format(task_running_time)) rospy.loginfo(" - Elapsed time : {}".format(task_elapsed_time)) rospy.loginfo_throttle_identical(5.0, "Current task score : {:.2f}".format(task_current_score))
def _move_base_status_callback(self, status_array): # Check if there's any goal active in the action server def is_active(status): return (status == GoalStatus.ACTIVE) or ( status == GoalStatus.PREEMPTING) or (status == GoalStatus.RECALLING) a_goal_is_active = False for entry in status_array.status_list: a_goal_is_active = a_goal_is_active or is_active(entry.status) if a_goal_is_active: rospy.loginfo_throttle_identical( 3.0, "A goal is active, station keeping is disabled") else: rospy.loginfo_throttle_identical( 3, "Station keeping enabled%s" % (", but no goal set" if not self._framing_box_corners else "")) self._station_keeping_is_enabled = not a_goal_is_active
def update(self): mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) if mission_plan is None: rospy.logwarn_throttle(5, "Mission plan was None!") return pt.Status.FAILURE if not self.do_not_visit: mission_plan.visit_wp() next_action = mission_plan.get_current_wp() if next_action is None: self.feedback_message = "Next action was None" rospy.logwarn_throttle(5, "Mission is complete:{}".format(mission_plan.is_complete())) return pt.Status.FAILURE rospy.loginfo_throttle_identical(5, "Set CURRENT_PLAN_ACTION {} to: {}".format(self.do_not_visit, str(next_action))) self.bb.set(bb_enums.CURRENT_PLAN_ACTION, next_action) return pt.Status.SUCCESS
def update(self): current_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ) if current_plan is None: # there is no plan, it can not change self.last_known_id = None self.last_known_time = 0 self.feedback_message = "There was no plan, plan is not changed" rospy.loginfo_throttle_identical(10, self.feedback_message) return pt.Status.SUCCESS if self.last_known_id is None: # this is the first plan we saw # record it, and let the tree tick again self.last_known_id = current_plan.plan_id self.last_known_time = current_plan.creation_time self.feedback_message = "First time seeing any plan, plan is changed" rospy.loginfo_throttle_identical(10, self.feedback_message) return pt.Status.FAILURE if self.last_known_id == current_plan.plan_id and self.last_known_time < current_plan.creation_time: # this is the same plan, but it was sent again, this means a restart # so the plan IS changed self.feedback_message = "Same plan_id, but the current plan is newer, plan is changed" rospy.loginfo_throttle_identical(10, self.feedback_message) self.last_known_id = current_plan.plan_id self.last_known_time = current_plan.creation_time self.bb.set(bb_enums.PLAN_IS_GO, False) return pt.Status.FAILURE if self.last_known_id != current_plan.plan_id: # the plan has been changed completely self.feedback_message = "A new plan is received!" rospy.loginfo_throttle_identical(10, self.feedback_message) self.last_known_id = current_plan.plan_id self.last_known_time = current_plan.creation_time self.bb.set(bb_enums.PLAN_IS_GO, False) return pt.Status.FAILURE self.feedback_message = "last_id:{}, current_id:{}".format( self.last_known_id, current_plan.plan_id) rospy.loginfo_throttle_identical(10, self.feedback_message) return pt.Status.SUCCESS
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 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 toggle(self, enable_): #function that toggles the service, that can be called from the code ret = self.toggle_ctrl_service(enable_) if ret.success: rospy.loginfo_throttle_identical(5, "Controller toggled")
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 _station_keeping(self): # do nothing, it'll take care of itself rospy.loginfo_throttle_identical( 2.0, "Station keeping, no more execution stages")
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("Goal received") #success = True self.nav_goal = goal.waypoint_pose.pose self.nav_goal_frame = goal.waypoint_pose.header.frame_id if self.nav_goal_frame is None or self.nav_goal_frame == '': rospy.logwarn("Goal has no frame id! Using utm by default") self.nav_goal_frame = 'utm' #'utm' self.nav_goal.position.z = goal.travel_depth # assign waypoint depth from neptus, goal.z is 0. if goal.speed_control_mode == 2: self.vel_ctrl_flag = 1 # check if NEPTUS sets a velocity elif goal.speed_control_mode == 1: self.vel_ctrl_flag = 0 # use RPM ctrl goal_point = PointStamped() goal_point.header.frame_id = self.nav_goal_frame goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z try: goal_point_local = self.listener.transformPoint( self.nav_goal_frame, goal_point) self.nav_goal.position.x = goal_point_local.point.x self.nav_goal.position.y = goal_point_local.point.y self.nav_goal.position.z = goal_point_local.point.z except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("Not transforming point to world local") pass rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x) r = rospy.Rate(11.) # 10hz counter = 0 while not rospy.is_shutdown() and self.nav_goal is not None: #self.toggle_yaw_ctrl.toggle(True) #self.toggle_depth_ctrl.toggle(True) # Preempted if self._as.is_preempt_requested(): rospy.loginfo('%s: Preempted' % self._action_name) #success = False self.nav_goal = None # Stop thrusters rpm1 = ThrusterRPM() rpm2 = ThrusterRPM() rpm1.rpm = 0 rpm2.rpm = 0 self.rpm1_pub.publish(rpm1) self.rpm2_pub.publish(rpm2) self.toggle_yaw_ctrl.toggle(False) self.toggle_depth_ctrl.toggle(False) self.toggle_vbs_ctrl.toggle(False) self.toggle_speed_ctrl.toggle(False) self.toggle_roll_ctrl.toggle(False) print('wp depth action planner: stopped thrusters') self._as.set_preempted(self._result, "Preempted WP action") return # Publish feedback if counter % 5 == 0: try: (trans, rot) = self.listener.lookupTransform( self.nav_goal_frame, self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("Error with tf:" + str(self.nav_goal_frame) + " to " + str(self.base_frame)) continue pose_fb = PoseStamped() pose_fb.header.frame_id = self.nav_goal_frame pose_fb.pose.position.x = trans[0] pose_fb.pose.position.y = trans[1] pose_fb.pose.position.z = trans[2] #self._feedback.feedback.pose = pose_fb #self._feedback.feedback.pose.header.stamp = rospy.get_rostime() #self._as.publish_feedback(self._feedback) #rospy.loginfo("Sending feedback") crosstrack_flag = 1 # set to 1 if we want to include crosstrack error, otherwise it computes a heading based on the next waypoint position if crosstrack_flag: #considering cross-track error according to Fossen, Page 261 eq 10.73,10.74 x_goal = self.nav_goal.position.x y_goal = self.nav_goal.position.y #checking if there is a previous WP, if there is no previous WP, it considers the current position if self.y_prev == 0 and self.x_prev == 0: self.y_prev = pose_fb.pose.position.y self.x_prev = pose_fb.pose.position.x y_prev = self.y_prev #read previous WP x_prev = self.x_prev #considering cross-track error according to Fossen, Page 261 eq 10.73,10.74 err_tang = math.atan2(y_goal - y_prev, x_goal - x_prev) # path tangential vector err_crosstrack = -(pose_fb.pose.position.x - x_prev) * math.sin(err_tang) + ( pose_fb.pose.position.y - y_prev) * math.cos( err_tang) # crosstrack error lookahead = 3 #lookahead distance(m) err_velpath = math.atan2(-err_crosstrack, lookahead) yaw_setpoint = (err_tang) + (err_velpath) rospy.loginfo_throttle_identical( 5, "Using Crosstrack Error, err_tang =" + str(err_tang) + "err_velpath" + str(err_velpath)) else: #Compute yaw setpoint based on waypoint position and current position xdiff = self.nav_goal.position.x - pose_fb.pose.position.x ydiff = self.nav_goal.position.y - pose_fb.pose.position.y #yaw_setpoint = 1.57-math.atan2(ydiff,xdiff) #The original yaw setpoint! yaw_setpoint = math.atan2(ydiff, xdiff) #print('xdiff:',xdiff,'ydiff:',ydiff,'yaw_setpoint:',yaw_setpoint) #compute yaw_error (e.g. for turbo_turn) #yaw_error= -(self.yaw_feedback - yaw_setpoint) #yaw_error= self.angle_wrap(yaw_error) #wrap angle error between -pi and pi #TODO Add logic for depth control with services here! depth_setpoint = self.nav_goal.position.z #depth_setpoint = goal.travel_depth #rospy.loginfo("Depth setpoint: %f", depth_setpoint) #Diving logic to use VBS at low speeds below 0.5 m/s self.toggle_depth_ctrl.toggle(True) self.toggle_vbs_ctrl.toggle(False) self.depth_pub.publish(depth_setpoint) self.vel_ctrl_flag = 0 #use constant rpm if self.vel_ctrl_flag: # if speed control is activated from neptus #if goal.speed_control_mode == 2: rospy.loginfo_throttle_identical( 5, "Neptus vel ctrl, no turbo turn") #with Velocity control self.toggle_yaw_ctrl.toggle(True) self.toggle_speed_ctrl.toggle(False) self.toggle_roll_ctrl.toggle(False) self.yaw_pub.publish(yaw_setpoint) # Publish to velocity controller travel_speed = goal.travel_speed self.toggle_speed_ctrl.toggle(True) #self.vel_pub.publish(self.vel_setpoint) self.vel_pub.publish(travel_speed) self.toggle_roll_ctrl.toggle(True) self.roll_pub.publish(self.roll_setpoint) #rospy.loginfo("Velocity published") else: # use rpm control instead of velocity control self.toggle_yaw_ctrl.toggle(True) self.yaw_pub.publish(yaw_setpoint) # Thruster forward rpm1 = ThrusterRPM() rpm2 = ThrusterRPM() rpm1.rpm = self.forward_rpm rpm2.rpm = self.forward_rpm self.rpm1_pub.publish(rpm1) self.rpm2_pub.publish(rpm2) #rospy.loginfo("Thrusters forward") counter += 1 r.sleep() # Stop thruster self.toggle_speed_ctrl.toggle(False) self.toggle_roll_ctrl.toggle(False) #self.vel_pub.publish(0.0) #self.roll_pub.publish(0.0) rpm1 = ThrusterRPM() rpm2 = ThrusterRPM() rpm1.rpm = 0 rpm2.rpm = 0 self.rpm1_pub.publish(rpm1) self.rpm2_pub.publish(rpm2) if self._result.reached_waypoint: #turbo turn at POI self.toggle_yaw_ctrl.toggle(False) self.toggle_speed_ctrl.toggle(False) self.toggle_roll_ctrl.toggle(False) self.toggle_depth_ctrl.toggle(False) self.toggle_vbs_ctrl.toggle(True) self.vbs_pub.publish(depth_setpoint) pitch_setpoint = -0.4 self.toggle_pitch_ctrl.toggle(True) self.lcg_pub.publish(pitch_setpoint) angle_err = 0 count = 0 initial_yaw = self.yaw_feedback while angle_err < 6.0 and count < 10: rospy.loginfo_throttle_identical( 5, 'Turbo-turning at POI!' + str(count)) self.turbo_turn(angle_err) angle_err = np.abs(initial_yaw - self.yaw_feedback) count = count + 1 # Stop thruster self.toggle_speed_ctrl.toggle(False) self.vel_pub.publish(0.0) rpm1 = ThrusterRPM() rpm2 = ThrusterRPM() rpm1.rpm = 0 rpm2.rpm = 0 self.rpm1_pub.publish(rpm1) self.rpm2_pub.publish(rpm2) #Stop controllers self.toggle_yaw_ctrl.toggle(False) self.toggle_depth_ctrl.toggle(False) self.toggle_vbs_ctrl.toggle(False) self.toggle_speed_ctrl.toggle(False) self.toggle_roll_ctrl.toggle(False) rospy.loginfo('%s: Succeeded' % self._action_name) #self.x_prev = self.nav_goal.position.x #self.y_prev = self.nav_goal.position.y #self._result.reached_waypoint= True self._as.set_succeeded(self._result, "WP Reached")
def execute_cb(self, goal): rospy.loginfo("Goal received") success = True self.nav_goal = goal.target_pose.pose self.nav_goal_frame = goal.target_pose.header.frame_id if self.nav_goal_frame is None or self.nav_goal_frame == '': rospy.logwarn("Goal has no frame id! Using utm by default") self.nav_goal_frame = 'utm' goal_point = PointStamped() goal_point.header.frame_id = self.nav_goal_frame goal_point.header.stamp = rospy.Time(0) goal_point.point.x = self.nav_goal.position.x goal_point.point.y = self.nav_goal.position.y goal_point.point.z = self.nav_goal.position.z try: goal_point_local = self.listener.transformPoint(self.nav_goal_frame, goal_point) self.nav_goal.position.x = goal_point_local.point.x self.nav_goal.position.y = goal_point_local.point.y self.nav_goal.position.z = goal_point_local.point.z except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print ("Not transforming point to world local") pass rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x) r = rospy.Rate(11.) # 10hz counter = 0 while not rospy.is_shutdown() and self.nav_goal is not None: 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 self.nav_goal = None # Stop thrusters rpm = DualThrusterRPM() rpm.thruster_front.rpm = 0. rpm.thruster_back.rpm = 0. self.rpm_pub.publish(rpm) self.yaw_pid_enable.publish(False) self.depth_pid_enable.publish(False) self.vbs_pid_enable.publish(False) self.vel_pid_enable.publish(False) print('wp depth action planner: stopped thrusters') self._as.set_preempted(self._result, "Preempted WP action") return # Publish feedback if counter % 5 == 0: try: (trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("Error with tf:"+str(self.nav_goal_frame) + " to "+str(self.base_frame)) continue pose_fb = PoseStamped() pose_fb.header.frame_id = self.nav_goal_frame pose_fb.pose.position.x = trans[0] pose_fb.pose.position.y = trans[1] pose_fb.pose.position.z = trans[2] self._feedback.base_position = pose_fb self._feedback.base_position.header.stamp = rospy.get_rostime() self._as.publish_feedback(self._feedback) #rospy.loginfo("Sending feedback") #Compute yaw setpoint. xdiff = self.nav_goal.position.x - pose_fb.pose.position.x ydiff = self.nav_goal.position.y - pose_fb.pose.position.y yaw_setpoint = math.atan2(ydiff,xdiff) print('xdiff:',xdiff,'ydiff:',ydiff,'yaw_setpoint:',yaw_setpoint) #compute yaw_error (e.g. for turbo_turn) yaw_error= -(self.yaw_feedback - yaw_setpoint) yaw_error= self.angle_wrap(yaw_error) #wrap angle error between -pi and pi depth_setpoint = self.nav_goal.position.z self.depth_pub.publish(depth_setpoint) #self.vbs_pid_enable.publish(False) #self.vbs_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: if self.turbo_turn_flag: #if turbo turn is included rospy.loginfo("Yaw error: %f", yaw_error) if abs(yaw_error) > self.turbo_angle_min and abs(yaw_error) < self.turbo_angle_max: #turbo turn with large deviations, maximum deviation is 3.0 radians to prevent problems with discontinuities at +/-pi self.yaw_pid_enable.publish(False) self.turbo_turn(yaw_error) self.depth_pid_enable.publish(False) self.vbs_pid_enable.publish(True) self.vbs_pub.publish(depth_setpoint) else: rospy.loginfo_throttle_identical(5,"Normal WP following") #normal turning if the deviation is small self.vbs_pid_enable.publish(False) self.depth_pid_enable.publish(True) self.yaw_pid_enable.publish(True) self.yaw_pub.publish(yaw_setpoint) self.create_marker(yaw_setpoint,depth_setpoint) # Thruster forward rpm = DualThrusterRPM() rpm.thruster_front.rpm = self.forward_rpm rpm.thruster_back.rpm = self.forward_rpm self.rpm_pub.publish(rpm) #rospy.loginfo("Thrusters forward") 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) self.create_marker(yaw_setpoint,depth_setpoint) # Thruster forward rpm = DualThrusterRPM() rpm.thruster_front.rpm = self.forward_rpm rpm.thruster_back.rpm = self.forward_rpm self.rpm_pub.publish(rpm) #rospy.loginfo("Thrusters forward") counter += 1 r.sleep() # Stop thruster self.vel_pid_enable.publish(False) rpm = DualThrusterRPM() rpm.thruster_front.rpm = 0.0 rpm.thruster_back.rpm = 0.0 self.rpm_pub.publish(rpm) #Stop controllers self.yaw_pid_enable.publish(False) self.depth_pid_enable.publish(False) self.vbs_pid_enable.publish(False) self.vel_pid_enable.publish(False) rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded(self._result)