def foot_scale_cb(self, request):
#FootScaleRequest):
# save messages
self.save_msgs = True
while len(self.last_msgs) < 100:
rospy.sleep(0.1)
rospy.logerr_throttle(0.5,
"Collecting messages for Scale correction")
self.save_msgs = False
rospy.sleep(0.1) # process last requests for saving messages
# this is a bit inefficient but it works
message_data = np.zeros((8, len(self.last_msgs)))
for i in range(len(self.last_msgs)):
message_data[0][i] = self.last_msgs[i].l_l_f
message_data[1][i] = self.last_msgs[i].l_l_b
message_data[2][i] = self.last_msgs[i].l_r_f
message_data[3][i] = self.last_msgs[i].l_r_b
message_data[4][i] = self.last_msgs[i].r_l_f
message_data[5][i] = self.last_msgs[i].r_l_b
message_data[6][i] = self.last_msgs[i].r_r_f
message_data[7][i] = self.last_msgs[i].r_r_b
single_scale = request.weight / (
np.mean(message_data[request.sensor]) - self.zero[request.sensor])
rospy.logwarn("mean:" + str(np.median(message_data[request.sensor])))
self.scale[request.sensor] = float(single_scale)
rospy.set_param("~scale", self.scale)
self.last_msgs = []
self.save_yaml()
return FootScaleResponse()
def zero_cb(self, request):
#EmptyRequest):
# save messages
self.save_msgs = True
while len(self.last_msgs) < 100:
rospy.sleep(0.1)
rospy.logerr_throttle(0.5, "Collecting messages for Zeroing")
self.save_msgs = False
rospy.sleep(0.1) # process last requests for saving messages
message_data = np.zeros((8, len(self.last_msgs)))
for i in range(len(self.last_msgs)):
message_data[0][i] = self.last_msgs[i].l_l_f
message_data[1][i] = self.last_msgs[i].l_l_b
message_data[2][i] = self.last_msgs[i].l_r_f
message_data[3][i] = self.last_msgs[i].l_r_b
message_data[4][i] = self.last_msgs[i].r_l_f
message_data[5][i] = self.last_msgs[i].r_l_b
message_data[6][i] = self.last_msgs[i].r_r_f
message_data[7][i] = self.last_msgs[i].r_r_b
self.last_msgs = []
for i in range(8):
self.zero[i] = (float(np.median(message_data[i])))
rospy.set_param("~zero", self.zero)
self.save_yaml()
return EmptyResponse()
def callback(self, rgb, depth):
if depth.encoding == '32FC1':
depth_32 = self._cv_bridge.imgmsg_to_cv2(depth) * 1000
depth_cv = np.array(depth_32, dtype=np.uint16)
elif depth.encoding == '16UC1':
depth_cv = self._cv_bridge.imgmsg_to_cv2(depth)
else:
rospy.logerr_throttle(
1, 'Unsupported depth type. Expected 16UC1 or 32FC1, got {}'.
format(depth.encoding))
return
rgb_cv = self._cv_bridge.imgmsg_to_cv2(rgb, 'bgr8')
labels, probs = self._hand_segmentation.segment(rgb_cv, depth_cv)
kernel = np.ones((3, 3), np.uint8)
cv2.erode(labels, kernel, labels)
idx = (labels == 1)
rgb_cv[idx] = [0, 0, 255]
overlay_msg = self._cv_bridge.cv2_to_imgmsg(rgb_cv)
overlay_msg.header.stamp = rospy.Time.now()
overlay_msg.header.frame_id = rgb.header.frame_id
overlay_msg.encoding = 'bgr8'
self._overlay_pub.publish(overlay_msg)
def controller_cb(self, j):
if not self.initialized:
return
if len(j.data) != 2:
rospy.logerr_throttle(0.5, 'Array size should be {}',
len(self.joint_ids))
return
new_pos = {}
new_pos_print = {}
vel = {}
# > 360
max_dist = 0 # deg
max_vel = 114.0 # RPM
max_t = 0 # sec
for idx, i in enumerate(self.joint_ids):
rospy.loginfo('cur_pos[{}]: {}'.format(
i, self.cur_pos[i].get() - self.zeros[i]))
for idx, i in enumerate(self.joint_ids):
new_pos[i] = j.data[idx] + self.zeros[i]
new_pos_print[i] = j.data[idx]
dist = math.fabs(self.cur_pos[i].get() - new_pos[i])
if dist > max_dist:
max_dist = dist
max_t = max_dist / (114.0)
rospy.loginfo('max_t: {}'.format(max_t))
for i in self.joint_ids:
if max_t:
vel[i] = math.fabs(self.cur_pos[i].get() - new_pos[i]) / max_t
else:
vel[i] = 0.0
rospy.loginfo('new_pos: {}'.format(new_pos_print))
rospy.loginfo('Vel: {}'.format(vel))
for i in self.joint_ids:
self.led[i].set(0)
is_error = False
try:
self.vel[i].set(vel[i])
self.pos[i].set(new_pos[i], deferred=True)
except BusError as e:
# Set LED
self.led[i].set(1)
# Don't move
self.pos[i].set(self.cur_pos[i].get())
rospy.logerr_throttle(0.5, e)
is_error = True
if not is_error:
self.bus.send_action()
def scan(self, callback):
"""Starts the DVL data collection.
This method is blocking, but calls callback at every ensemble.
To stop a scan, simply call the preempt() method. Otherwise, the scan
will run forever.
Args:
callback: Callback for feedback.
Called with args=(TeledyneNavigator, Ensemble).
"""
self._preempted = False
# Set PD5 output format.
self.write("PD5\n")
# Set earth coordinate transformation.
self.write("EX11111\n")
# Start pinging.
self.write("CS\n")
while not self._preempted:
# Preempt on ROS shutdown.
if rospy.is_shutdown():
self.preempt()
return
try:
ensemble = self._get_ensemble()
callback(self, ensemble)
except Exception as e:
rospy.logerr_throttle(1.0, str(e))
continue
def _callback_camera_info(self, camera_info):
if camera_info.K[0] == 0:
rospy.logerr_throttle(
5.0,
rospy.get_name() +
": Invalid CameraInfo received. Check your camera settings.")
self._camera_info = camera_info
def start_animation(self):
"""
This will NOT wait by itself. You have to check
animation_finished()
by yourself.
:return:
"""
first_try = self.blackboard.dynup_action_client.wait_for_server(
rospy.Duration(rospy.get_param("hcm/anim_server_wait_time", 1)))
if not first_try:
server_running = False
while not server_running and not self.blackboard.shut_down_request and not rospy.is_shutdown():
rospy.logerr_throttle(5.0,
"Dynup Action Server not running! Dynup cannot work without dynup server!"
"Will now wait until server is accessible!")
server_running = self.blackboard.dynup_action_client.wait_for_server(rospy.Duration(1))
if server_running:
rospy.logwarn("Dynup server now running, hcm will go on.")
else:
rospy.logwarn("Dynup server did not start.")
return False
goal = bitbots_msgs.msg.DynUpGoal()
goal.front = True # is currently ignored
self.blackboard.dynup_action_client.send_goal(goal)
return True
def _check_constraints(self):
for constraint in self.constraints:
source_topic = constraint['source_topic']
warn_delay = constraint['warn_delay']
error_delay = constraint['error_delay']
now = rospy.Time.now()
source_cb = self.topic_auto_subscriber_dict[source_topic]
source_timestamp = source_cb.get_timestamp()
if source_timestamp is not None:
delay = (now - source_timestamp).to_sec()
if delay >= error_delay:
rospy.logerr_throttle(
5,
"Time constraint not satisfied! {} Seconds passed after last {} Message!"
.format(delay, source_topic))
elif delay >= warn_delay:
rospy.logwarn_throttle(
5,
"Time constraint not satisfied! {} Seconds passed after last {} Message!"
.format(delay, source_topic))
else:
rospy.logwarn_throttle(
5, "Never received {}!".format(source_topic))
def get_inverse_kinematics(self, pose):
try:
rospy.wait_for_service('compute_ik', 2)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("Arm commander - Impossible to connect to IK service : " + str(e))
return False, []
try:
tool_link_pose = self.__transform_handler.tcp_to_ee_link_pose_target(pose, "tool_link")
moveit_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
req = PositionIKRequest()
req.group_name = self.__arm.get_name()
req.ik_link_name = self.__arm.get_end_effector_link()
req.robot_state.joint_state.name = self.__arm_state.joints_name
req.robot_state.joint_state.position = self.__arm_state.joint_states
req.pose_stamped.pose = tool_link_pose
response = moveit_ik(req)
except rospy.ServiceException as e:
rospy.logerr("Arm commander - Service call failed: {}".format(e))
return False, []
if response.error_code.val == MoveItErrorCodes.NO_IK_SOLUTION:
rospy.logerr_throttle(0.5, "Arm commander - MoveIt didn't find an IK solution")
return False, []
elif response.error_code.val != MoveItErrorCodes.SUCCESS:
rospy.logerr("Arm commander - IK Failed : code error {}".format(response.error_code.val))
return False, []
return True, list(response.solution.joint_state.position[:6])
def setpoint_publisher(mavros_interface_node,
commander_class_instance,
ros_rate=100):
"""
This method continuously publishes the setpoint state - must run at a deterministic rate to prevent offboard mode
from exiting (offboard mode exits if a new instruction is not received at a minimum of 2 hz)
"""
rate = rospy.Rate(ros_rate)
while not rospy.is_shutdown():
try:
# todo - add this thread lock to mission_states
# todo - use commander lock rather than mission states lock?
# with mavros_interface_node.setpoint_lock:
mavros_interface_node.setpoint_lock.acquire()
mavros_interface_node.local_pos_pub_raw.publish(
commander_class_instance.sp_raw)
mavros_interface_node.setpoint_lock.release()
except Exception as e:
rospy.logerr_throttle(
1, ('couldnt publish the setpoint message because: ', e))
try: # prevent garbage in console output when thread is killed
rate.sleep()
except rospy.ROSInterruptException:
pass
def check_joint_raw_sensor_value(self, initial_raw_value, joint,
dictionary):
status = ""
warning = False
test_failed = False
if joint.joint_name != self._hand_prefix + "_wrj1" and joint.joint_name != self._hand_prefix + "_thj5":
initial_raw_value = initial_raw_value[-1:]
for i, value in enumerate(initial_raw_value):
time = rospy.Time.now() + self._check_duration
while rospy.Time.now() < time and test_failed is not True:
difference = joint._raw_sensor_data[i] - initial_raw_value[i]
if abs(difference) <= SENSOR_CUTOUT_THRESHOLD:
if abs(difference) < NR_OF_BITS_NOISE_WARNING:
status = "{} bits noise - CHECK PASSED".format(
abs(difference) + 1)
elif abs(difference) == NR_OF_BITS_NOISE_WARNING:
rospy.logwarn_throttle(1,
"Found value with 3 bits diff")
status = "3 bits noise - WARNING"
test_failed = True
else:
rospy.logerr_throttle(
1, "Found value with {} bits diff".format(
abs(difference) + 1))
status = "{} bits noise - CHECK FAILED".format(
abs(difference) + 1)
test_failed = True
self._publishing_rate.sleep()
print("Finished loop for {}".format(joint.joint_name))
if joint.joint_name not in dictionary:
dictionary[joint.joint_name] = status
else:
name = joint.joint_name + "_1"
dictionary[name] = status
def spawn_drone(self):
"""
Spawn the drone.
"""
# 1. create spawn service handle
try:
client = rospy.ServiceProxy('/spawn', SpawnRobot)
except rospy.ServiceException as e:
rospy.logerr_throttle(1.0, 'Spawn Client Creation Failed : %s' % e)
# 2. obtain semi-hardcoded drone image
img = os.path.join(rospack.get_path('iarc_sim_engine'), 'data',
'drone.jpg')
# 3. actually spawn the drone
drone = self.spawn_robot(
client,
name=self.name,
img=img,
radius=cfg.DRONE_RADIUS,
pose=Pose2D(0, 0, cfg.PI / 2),
robot_class=Drone,
)
return drone
def start_animation(self, anim):
"""
This will NOT wait by itself. You have to check
animation_finished()
by yourself.
:param anim: animation to play
:return:
"""
rospy.loginfo("Playing animation " + anim)
if anim is None or anim == "":
rospy.logwarn("Tried to play an animation with an empty name!")
return False
first_try = self.blackboard.animation_action_client.wait_for_server(
rospy.Duration(1))
if not first_try:
server_running = False
while not server_running and not rospy.is_shutdown():
rospy.logerr_throttle(
5.0,
"Animation Action Server not running! Motion can not work without animation action server. "
"Will now wait until server is accessible!")
server_running = self.blackboard.animation_action_client.wait_for_server(
rospy.Duration(1))
if server_running:
rospy.logwarn("Animation server now running, hcm will go on.")
else:
rospy.logwarn("Animation server did not start.")
return False
goal = humanoid_league_msgs.msg.PlayAnimationGoal()
goal.animation = anim
goal.hcm = True # the animation is from the hcm
self.blackboard.animation_action_client.send_goal(goal)
return True
def publish_uav_target_pose():
try:
current_pose, target_pose = target_finder.extract_poses()
targetOdom = Odometry()
# Populating Odometry msg
global sequence_number
targetOdom.header.frame_id = "odom"
targetOdom.header.stamp = rospy.get_rostime()
targetOdom.header.seq = sequence_number
sequence_number += 1
x, y, theta = target_pose
q = transformations.quaternion_from_euler(0.0, 0.0, theta, axes='sxyz')
targetOdom.pose.pose.position.x = x
targetOdom.pose.pose.position.y = y
targetOdom.pose.pose.orientation.x = q[0]
targetOdom.pose.pose.orientation.y = q[1]
targetOdom.pose.pose.orientation.z = q[2]
targetOdom.pose.pose.orientation.w = q[3]
uav_target_odom_pub.publish(targetOdom)
targetPose = PoseWithCovarianceStamped()
targetPose.header = targetOdom.header
targetPose.pose.pose = targetOdom.pose.pose
uav_target_pose_pub.publish(targetPose)
except ValueError as ex:
rospy.logerr_throttle(5.0, rospy.get_name() + " : " + str(ex))
def updateTfs(self, event=None):
rospy.logdebug("updateTfs")
model_states = self.getWorldState()
Tr_m = self.getRobotTransform(model_states=model_states)
if Tr_m is None:
return None
try:
To_r = self.tfBuffer.lookup_transform("base_footprint", "odom",
Tr_m.header.stamp,
rospy.Duration(.1))
#Approach inspired by https://answers.ros.org/question/215656/how-to-transform-a-pose/?answer=215666#post-id-215666
To_r_mat = self.transformToMat(To_r)
Tr_m_mat = self.transformToMat(Tr_m)
Tm_o_mat = Tr_m_mat.dot(To_r_mat)
Tm_o = self.matToTransform(Tm_o_mat)
Tm_o.header.frame_id = "map"
Tm_o.header.stamp = To_r.header.stamp
Tm_o.child_frame_id = "odom"
self.tf_broadcaster_m.sendTransform(Tm_o)
self.transform = Tm_o
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logerr_throttle(1, "can't find odom to base")
def _publish_area(self):
try:
if self._sim2d:
t, _ = self._tf.lookupTransform('map', 'base_link',
rospy.Time(0))
x, y, z = t
r = z * np.tan(np.pi / 3)
th = np.linspace(-np.pi, np.pi)
ar_x = x + r * np.cos(th)
ar_y = y + r * np.sin(th)
ar = np.stack([ar_x, ar_y, 0 * ar_y], axis=-1)
poly = self._a2p(ar)
self._ar_msg.header.stamp = rospy.Time.now()
self._ar_msg.header.frame_id = 'map'
self._ar_msg.polygon = poly
self._ar_pub.publish(self._ar_msg)
elif self._cam_frame:
# looks a bit stupid ... TODO(yoonyoungcho): fix
t, q = self._tf.lookupTransform(self._map_frame,
self._cam_frame, rospy.Time(0))
q = [q[3], q[0], q[1], q[2]]
ar = observability(self._K, self._w, self._h, q, t, False)
poly = self._a2p(ar)
self._ar_msg.header.stamp = rospy.Time.now()
self._ar_msg.header.frame_id = self._map_frame
self._ar_msg.polygon = poly
self._ar_pub.publish(self._ar_msg)
except Exception as e:
rospy.logerr_throttle(
1.0,
'Exception at _publish_area() in f_rviz.py : {}'.format(e))
def initialize(self):
if self._initialized:
return
try:
for i in range(self._num_markers):
#M = tx.compose_matrix(translate = self._xyz[i], angles=self._rpy[i])
#txn = tx.translation_from_matrix(M)
#rxn = tx.quaternion_from_matrix(M)
txn = self._xyz[i]
rxn = tx.quaternion_from_euler(*(self._rpy[i]))
req = SetModelStateRequest()
req.model_state.model_name = 'a_{}'.format(i)
req.model_state.reference_frame = 'map' # TODO : wait for base_link for correctness. for now, ok.
fill_pose_msg(req.model_state.pose, txn, rxn)
res = self._gsrv(req)
if not res.success:
rospy.logerr_throttle(
1.0, 'Set Model Request Failed : {}'.format(
res.status_message))
return
except rospy.ServiceException as e:
rospy.logerr_throttle(1.0, 'Initialization Failed : {}'.format(e))
return
self._initialized = True
def inverse(self, t, TGB):
if len(t) < self._num_jnts:
rospy.logerr_throttle(
5,
"[ServoIk.inverse] Wrong number of joints published. Published: {} Existing: {}"
.format(len(t), self._num_jnts))
return None
else:
joints_array = kdl.JntArray(self._num_jnts)
for i in xrange(self._num_jnts):
joints_array[i] = t[i]
joint_velocities_array = kdl.JntArray(self._num_jnts)
FEB = kdl.Frame()
FGB = kdl.Frame(
kdl.Rotation.Quaternion(TGB[1][0], TGB[1][1], TGB[1][2],
TGB[1][3]),
kdl.Vector(TGB[0][0], TGB[0][1], TGB[0][2]))
self._fk_p.JntToCart(joints_array, FEB)
FEGB = FGB * FEB.Inverse()
iterations = 0
while (FEGB.p.Norm() > LINEAR_TOLERANCE or FEGB.M.GetRotAngle()[0] > ANGULAR_TOLERANCE) \
and iterations < MAX_ITERATIONS:
iterations += 1
twist = kdl.Twist(FEGB.p, FEGB.M.GetRot())
self._ik_v.CartToJnt(joints_array, twist,
joint_velocities_array)
maximum = 0.0
for i in xrange(self._num_jnts):
if joint_velocities_array[i] > maximum:
maximum = joint_velocities_array[i]
if maximum > SATURATION:
reduction = SATURATION / maximum
else:
reduction = 1.0
for i in xrange(self._num_jnts):
joints_array[i] += reduction * joint_velocities_array[i]
self._fk_p.JntToCart(joints_array, FEB)
FEGB = FGB * FEB.Inverse()
logdebug('amount of iterations needed: %s', iterations)
if iterations >= MAX_ITERATIONS:
return None
solution = []
for i in xrange(self._num_jnts):
while joints_array[i] < pi:
joints_array[i] += 2.0 * pi
while joints_array[i] > pi:
joints_array[i] -= 2.0 * pi
if joints_array[i] < self.lower_limits[i] \
or joints_array[i] > self.upper_limits[i]:
return None
solution.append(joints_array[i])
return solution
def __check_alarms(self):
"""Check alarms, if there is an alarm, check that all alarms are
in whitelist, and reset if so.
Returns:
True if no alarms found, False if alarms were found (regardless
of whether reset was successful)
"""
alarms = self.__alarm_interface.alarms
unrecognized_alarms = [
alm for alm in alarms
if alm.alarm_identifier not in self.__alrm_whitelist
]
if len(alarms) >= NUM_ALARMS:
rospy.logwarn(
'There are the max number of %d alarms on the '
'robot. Potentially missing some alarms', NUM_ALARMS)
if len(alarms) > 0:
rospy.logdebug('Alarms on Robot: %s', alarms)
if len(unrecognized_alarms) == 0:
rospy.loginfo('Alarm in whitelist, resetting')
with self.__config_lock:
self.__io_interface.reset()
else:
rospy.logerr_throttle(
5, 'At least one alarm is not in whitelist, check teach '
'pendant. All alarms: %s' % (alarms, ))
return len(alarms) == 0
def precondition_check(self):
# check we are landed
# todo - if we are already airborne then passthrough this state
# todo - if in gps mode then take an average of readings for the start x,y and z data?
if self._prerun_complete:
try:
# rospy.sleep(1.0)
self.start_x = copy(self._ros_message_node.local_x)
self.start_y = copy(self._ros_message_node.local_y)
self.start_z = copy(self._ros_message_node.local_z)
if self.heading_tgt_rad is None:
self.heading_tgt_rad = copy(
self._ros_message_node.yaw_local)
self.preconditions_satisfied = True
rospy.loginfo('takeoff target: {} start z: {}'.format(
self.to_altitude_tgt, self.start_z))
self.vel_ramp_time_start = rospy.Time.now().to_sec()
except Exception as e:
rospy.logerr_throttle(
1, 'some exception {} happened in 2323jj3'.format(e))
def __connect(self, retry):
"""
connect with ble device through gatttool, return a pexpect object
"""
while True:
try:
gt = pexpect.spawn("gatttool -b " + self.mac +
" -t random --char-write-req -a " +
self.write_handle + " -n 0100 --listen")
connect_expect = "Characteristic value was written successfully"
gt.expect(connect_expect, timeout=10)
break
except (pexpect.TIMEOUT, pexpect.EOF) as e:
rospy.logerr_throttle(
10,
rospy.get_name() +
": Failed to connect to device, please check connection and handle settings"
)
if (retry):
rospy.logwarn_throttle(10, "Reconnecting...")
else:
exit(0)
except KeyboardInterrupt:
rospy.loginfo(
rospy.get_name() +
": Received keyboard interrupt. Quitting cleanly.")
retry = False
exit(0)
if (not retry):
break
return gt
def handle_gps(msg, args):
"""
Publish the GPS position in waterlinked frame.
"""
pub, tf = args
e, n, a, zone, band = lla_to_utm(msg.latitude,
msg.longitude,
0.0,
radians=False)
rospy.logdebug("utm: {} {} {}".format(e, n, a))
p_utm = PointStamped(Header(0, rospy.Time.from_sec(0.0), "utm"),
Point(e, n, a))
# tf = Buffer()
try:
p_wl = tf.transform(p_utm, "waterlinked")
rospy.logdebug("waterlinked: {} {} {}".format(p_wl.point.x,
p_wl.point.y,
p_wl.point.z))
except Exception as e:
rospy.logerr_throttle(5.0, "{} | {}".format(rospy.get_name(),
e.message))
return
msg_pose_with_cov_stamped = PoseWithCovarianceStamped()
var_xyz = pow(1.5, 2) # calculate variance from standard deviation
msg_pose_with_cov_stamped.header.stamp = msg.header.stamp
msg_pose_with_cov_stamped.header.frame_id = p_wl.header.frame_id
msg_pose_with_cov_stamped.pose.pose.position.x = p_wl.point.x
msg_pose_with_cov_stamped.pose.pose.position.y = p_wl.point.y
msg_pose_with_cov_stamped.pose.pose.position.z = p_wl.point.z
msg_pose_with_cov_stamped.pose.pose.orientation = Quaternion(0, 0, 0, 1)
msg_pose_with_cov_stamped.pose.covariance = [
var_xyz, 0, 0, 0, 0, 0, 0, var_xyz, 0, 0, 0, 0, 0, 0, var_xyz, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
pub.publish(msg_pose_with_cov_stamped)
def callback_rgbd(self, rgb, depth):
if depth.encoding == '32FC1':
depth_cv = self.cv_bridge.imgmsg_to_cv2(depth)
elif depth.encoding == '16UC1':
depth_cv = self.cv_bridge.imgmsg_to_cv2(depth).copy().astype(np.float32)
depth_cv /= 1000.0
else:
rospy.logerr_throttle(
1, 'Unsupported depth type. Expected 16UC1 or 32FC1, got {}'.format(
depth.encoding))
return
im = self.cv_bridge.imgmsg_to_cv2(rgb, 'bgr8')
# rescale image if necessary
if cfg.TEST.SCALES_BASE[0] != 1:
im_scale = cfg.TEST.SCALES_BASE[0]
im = pad_im(cv2.resize(im, None, None, fx=im_scale, fy=im_scale, interpolation=cv2.INTER_LINEAR), 16)
depth_cv = pad_im(cv2.resize(depth_cv, None, None, fx=im_scale, fy=im_scale, interpolation=cv2.INTER_NEAREST), 16)
with lock:
self.im = im.copy()
self.depth = depth_cv.copy()
self.rgb_frame_id = rgb.header.frame_id
self.rgb_frame_stamp = rgb.header.stamp
def yPositionCheck(self):
# Position constraints are set in vector_field_planner_params.yaml
# y_constraint: [ymin, ymax]
if np.any([self.y_constraint[0] > self.pos[1], self.pos[1] > self.y_constraint[1]]):
rospy.logerr_throttle(1,"Unsafe Y Position: Y=%.2f" %(self.pos[1]))
return False
return True
def controller(self):
if (rospy.get_time() - self.pose_msg_time > self.max_msg_timeout):
rospy.logwarn_throttle(10.0, "No pose received!")
return 0.0
if (rospy.get_time() - self.twist_msg_time > self.max_msg_timeout):
rospy.logwarn_throttle(10.0, "No twist received!")
return 0.0
if self.controller_type is None:
rospy.logwarn_throttle(10.0, "No controller chosen!")
return 0.0
# return 0.0 if setpoint is 'unsafe'
if ((self.desired_x_pos < self.min_x_limit)
or (self.desired_x_pos > self.max_x_limit)):
rospy.logwarn_throttle(10.0, "x setpoint outside safe region!")
return 0.0
# return 0.0 if setpoint velocity is 'unsafe'
if ((self.desired_x_velocity < -self.x_d_limit)
or (self.desired_x_velocity > self.x_d_limit)):
rospy.logwarn_throttle(10.0,
"x velocity setpoint outside safe region!")
return 0.0
# return 0.0 if x_pos is 'unsafe'
if ((self.current_x_pos < self.min_x_limit)
or (self.current_x_pos > self.max_x_limit)):
rospy.logwarn_throttle(10.0, "x outside safe region!")
return 0.0
if self.x_uncertainty > 1.0:
return 0
if self.controller_type == 0:
# SMC
self.e1 = self.desired_x_pos - self.current_x_pos
self.e2 = self.desired_x_velocity - self.current_x_velocity
s = self.e2 + self.Lambda * self.e1
k = 10**(-self.k_u * self.x_uncertainty)
u = self.alpha * (self.desired_x_acceleration +
self.Lambda * self.e2 + self.kappa *
(s / (abs(s) + self.epsilon)))
elif self.controller_type == 1:
# PD-Controller
self.e1 = self.desired_x_pos - self.current_x_pos
self.e2 = self.desired_x_velocity - self.current_x_velocity
u = self.k_p * self.e1 + self.k_d * self.e2
else:
rospy.logerr_throttle(10.0,
"\nError! Undefined Controller chosen.\n")
return 0.0
k = 10**(-self.k_u * self.x_uncertainty)
return self.sat(k * u)
def initialise(self):
if not self.action_server_ok:
rospy.logwarn_throttle(5, "No action server found for A_GotoWaypoint!")
return
mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if mission_plan is None:
rospy.logwarn("No mission plan found!")
return
wp = mission_plan.get_current_wp()
# wp = self.bb.get(bb_enums.CURRENT_PLAN_ACTION)
# # if this is the first ever action, we need to get it ourselves
if wp is None:
rospy.logwarn("No wp found to execute! Does the plan have any waypoints that we understand?")
return
if wp.tf_frame != self.goal_tf_frame:
rospy.logerr_throttle(5, 'The frame of the waypoint({0}) does not match the expected frame({1}) of the action client!'.format(frame, self.goal_tf_frame))
return
if wp.maneuver_id == imc_enums.MANEUVER_SAMPLE:
# TODO change this behaviour lol
rospy.logwarn(5, "THIS IS A SAMPLE MANEUVER, WE ARE USING SIMPLE GOTO FOR THIS!!!")
# construct the message
goal = GotoWaypointGoal()
goal.waypoint_pose.pose.position.x = wp.x
goal.waypoint_pose.pose.position.y = wp.y
goal.goal_tolerance = self.goal_tolerance
# 0=None, 1=Depth, 2=Altitude in the action
# thankfully these are the same in IMC and in the Action
# but Action doesnt have 'height'
if wp.z_unit == imc_enums.Z_HEIGHT:
wp.z_unit = imc_enums.Z_NONE
goal.z_control_mode = wp.z_unit
goal.travel_depth = wp.z
# 0=None, 1=RPM, 2=speed in the action
# 0=speed, 1=rpm, 2=percentage in IMC
if wp.speed_unit == imc_enums.SPEED_UNIT_RPM:
goal.speed_control_mode = GotoWaypointGoal.SPEED_CONTROL_RPM
goal.travel_rpm = wp.speed
elif wp.speed_unit == imc_enums.SPEED_UNIT_MPS:
goal.speed_control_mode = GotoWaypointGoal.SPEED_CONTROL_SPEED
goal.travel_speed = wp.speed
else:
goal.speed_control_mode = GotoWaypointGoal.SPEED_CONTROL_NONE
rospy.logwarn_throttle(1, "Speed control of the waypoint action is NONE!")
self.action_goal = goal
rospy.loginfo(">>> Goto waypoint action goal initialized:"+str(goal))
# ensure that we still need to send the goal
self.sent_goal = False
def package(self) -> Optional[Dict]:
if not self.last_reading or not issubclass(self.msg, Messages.string):
return None
try:
return json.loads(self.last_reading.data)
except Exception as e:
rospy.logerr_throttle(
0.5, f'Parse package failed:\n{e}\n{self.last_reading}')
return None
def callback(self, request):
rgb = request.rgb
depth = request.depth_registered
response = skin_srvs.PredictHandsResponse()
# Kinect One / Nerf data logic
# Normally, we expect 32FC1 to contain the distance in meters.
# However, the recorded data from the Nerf experiments, which uses a Kinect
# One, gives millimeters as 32FC1.
is_nerf = False
if depth.encoding == '32FC1' and is_nerf:
depth_32 = self._cv_bridge.imgmsg_to_cv2(depth) + 0.5
depth_cv = np.array(depth_32, dtype=np.uint16)
elif depth.encoding == '32FC1':
# Standard Kinect 360: depth is 32FC1 as meters, which we conver to 16-bit millimeters
depth_32 = self._cv_bridge.imgmsg_to_cv2(depth) * 1000
depth_cv = np.array(depth_32, dtype=np.uint16)
elif depth.encoding == '16UC1':
depth_cv = self._cv_bridge.imgmsg_to_cv2(depth)
else:
rospy.logerr_throttle(
1, 'Unsupported depth type. Expected 32FC1 or 16UC1, got {}'.
format(depth.encoding))
return response
rgb_cv = self._cv_bridge.imgmsg_to_cv2(rgb, 'bgr8')
labels, probs = self._hand_segmentation.segment(rgb_cv, depth_cv)
kernel = np.ones((3, 3), np.uint8)
cv2.erode(labels, kernel, labels)
now = rospy.Time.now()
response.prediction = self._cv_bridge.cv2_to_imgmsg(labels)
response.prediction.header.stamp = now
response.prediction.header.frame_id = rgb.header.frame_id
response.prediction.encoding = 'mono8'
idx = (labels == 1)
rgb_cv[idx] = [0, 0, 255]
overlay_msg = self._cv_bridge.cv2_to_imgmsg(rgb_cv)
overlay_msg.header.stamp = rospy.Time.now()
overlay_msg.header.frame_id = rgb.header.frame_id
overlay_msg.encoding = 'bgr8'
self._label_pub.publish(overlay_msg)
if self.is_publishing_depth_cloud:
rgb.header.stamp = now
rgb.header.frame_id = self._info.header.frame_id
depth_msg = self._cv_bridge.cv2_to_imgmsg(depth_cv)
depth_msg.header.stamp = now
depth_msg.header.frame_id = self._info.header.frame_id
self._info.header.stamp = now
self._rgb_pub.publish(rgb)
self._depth_pub.publish(depth_msg)
self._info_pub.publish(self._info)
return response
def handle_link_states(self, data):
try:
idx = data.name.index("lander::l_scoop_tip")
except ValueError:
rospy.logerr_throttle(1, "lander::l_scoop_tip not found in link_states")
return
position = data.pose[idx].position
orientation = data.pose[idx].orientation
self.check_and_submit(position, orientation)
def set_PWM_callback(self, msg):
#data.pin, data.width
if not 0<=msg.pin<=31 or not (msg.width == 0 or 500<=msg.width<=2500):
rospy.logerr_throttle(1, "Malformed PWM message: %d | %d" % (msg.pin, msg.width))
return
#convert to duty cycle
hz = self.gpio.get_PWM_frequency(msg.pin)
duty = (1/hz)*10**10/msg.width #10^6*10^4
rospy.loginfo("Pin: %d, Duty: %d", msg.pin, duty)
self.gpio.set_PWM_dutycycle(msg.pin, duty)
def need_kill(self):
now = rospy.Time.now()
in_grace_period = now - self.LAUNCH_TIME < self.GRACE_PERIOD
odom_loss = now - self.last_time > self.TIMEOUT and not in_grace_period
if odom_loss:
rospy.logerr_throttle(1, 'LOST ODOM FOR {} SECONDS'.format((rospy.Time.now() - self.last_time).to_sec()))
self.ab.raise_alarm(
problem_description='LOST ODOM FOR {} SECONDS'.format((rospy.Time.now() - self.last_time).to_sec()),
severity=5
)
return odom_loss or self.odom_discontinuity
def wait_for_server(self):
"""Waits until interoperability server is reachable."""
reachable = False
rate = rospy.Rate(1)
while not reachable and not rospy.is_shutdown():
try:
response = requests.get(
self.url, timeout=self.timeout, verify=self.verify)
response.raise_for_status()
reachable = response.ok
except requests.ConnectionError:
rospy.logwarn_throttle(5.0, "Waiting for server: {}".format(
self.url))
except Exception as e:
rospy.logerr_throttle(
5.0, "Unexpected error waiting for server: {}, {}".format(
self.url, e))
if not reachable:
rate.sleep()
def test_log(self):
# we can't do anything fancy here like capture stdout as rospy
# grabs the streams before we do. Instead, just make sure the
# routines don't crash.
real_stdout = sys.stdout
real_stderr = sys.stderr
try:
try:
from cStringIO import StringIO
except ImportError:
from io import StringIO
sys.stdout = StringIO()
sys.stderr = StringIO()
import rospy
rospy.loginfo("test 1")
self.assert_("test 1" in sys.stdout.getvalue())
rospy.logwarn("test 2")
self.assert_("[WARN]" in sys.stderr.getvalue())
self.assert_("test 2" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logerr("test 3")
self.assert_("[ERROR]" in sys.stderr.getvalue())
self.assert_("test 3" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logfatal("test 4")
self.assert_("[FATAL]" in sys.stderr.getvalue())
self.assert_("test 4" in sys.stderr.getvalue())
# logXXX_throttle
for i in range(3):
sys.stdout = StringIO()
rospy.loginfo_throttle(3, "test 1")
if i == 0:
self.assert_("test 1" in sys.stdout.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stdout.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 1" in sys.stdout.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logwarn_throttle(3, "test 2")
if i == 0:
self.assert_("test 2" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 2" in sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logerr_throttle(3, "test 3")
if i == 0:
self.assert_("test 3" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 3" in sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logfatal_throttle(3, "test 4")
if i == 0:
self.assert_("test 4" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 4" in sys.stderr.getvalue())
finally:
sys.stdout = real_stdout
sys.stderr = real_stderr
