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)
