def make_cylinder_marker(self,
base,
length,
color,
frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]),
**kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(position=sub8_utils.numpy_to_point(center),
orientation=sub8_utils.numpy_to_quaternion(
[0.0, 0.0, 0.0, 1.0]))
marker = visualization_msgs.Marker(
ns='sub',
header=sub8_utils.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs)
return marker
def publish_target_pose(self, position, orientation):
self.target_pose_pub.publish(
PoseStamped(
header=sub8_utils.make_header('/map'),
pose=Pose(
position=sub8_utils.numpy_to_point(position),
orientation=sub8_utils.numpy_to_quaternion(orientation))))
def read_datagram(self):
msg = self.serial.read(
self._datagram_lengths[self.datagram_identifier])
start = 0
# gyr
gyro = np.fromstring(b'\x00' + msg[start + 0:start + 3][::-1] +
b'\x00' + msg[start + 3:start + 6][::-1] +
b'\x00' + msg[start + 6:start + 9][::-1],
dtype='<i').astype(np.float32) / (2**14)
self.last_msg = gyro
start += 10
# acc
linear_acceleration = np.fromstring(
b'\x00' + msg[start + 0:start + 3][::-1] + b'\x00' +
msg[start + 3:start + 6][::-1] + b'\x00' +
msg[start + 6:start + 9][::-1],
dtype='<i').astype(np.float32) / (2**19)
start += 10
# inc
inclination = np.fromstring(b'\x00' + msg[start + 0:start + 3][::-1] +
b'\x00' + msg[start + 3:start + 6][::-1] +
b'\x00' + msg[start + 6:start + 9][::-1],
dtype='<i').astype(np.float32) / (2**22)
imu_msg = Imu(header=sub8_ros_tools.make_header(),
angular_velocity=Vector3(*gyro),
linear_acceleration=Vector3(*linear_acceleration))
self.imu_pub.publish(imu_msg)
def request_buoy(self, srv):
print 'requesting', srv
response = self.find_single_buoy(np.copy(self.last_image), srv.target_name)
if response is False:
print 'did not find'
resp = VisionRequest2DResponse(
header=sub8_ros_tools.make_header(frame='/stereo_front/right'),
found=False
)
else:
# Fill in
center, radius = response
resp = VisionRequest2DResponse(
header=Header(stamp=self.last_image_time, frame_id='/stereo_front/right'),
pose=Pose2D(
x=center[0],
y=center[1],
),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True
)
return resp
def range_callback(self, msg):
'''Handle range data grabbed from dvl'''
# future: should be /base_link/dvl, no?
frame = '/dvl'
distance = msg.data
# Color a sharper red if we're in danger
if distance < 1.0:
color = (1.0, 0.1, 0.0, 0.9)
else:
color = (0.2, 0.8, 0.0, 0.7)
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=color, # red,
frame=frame,
up_vector=np.array([0.0, 0.0, -1.0]), # up is down in range world
id=1 # Keep these guys from overwriting eachother
)
self.depth_marker.ns='sub'
self.depth_marker.header = sub8_utils.make_header(frame='/dvl')
self.depth_marker.pose = marker.pose
self.depth_marker.text = str(round(distance, 3)) + 'm'
self.depth_marker.id = 1
self.rviz_pub_t.publish(self.depth_marker)
self.rviz_pub.publish(marker)
def request_bin(self, srv):
self.bin_type = srv.target_name
if (self.last_image != None):
print 'requesting', srv
response = self.find_single_bin(np.copy(self.last_image), srv.target_name)
if response is False:
print 'did not find'
resp = VisionRequest2DResponse(
header=sub8_ros_tools.make_header(frame='/down'),
found=False
)
else:
# Fill in
center, radius = response
resp = VisionRequest2DResponse(
header=Header(stamp=self.last_image_time, frame_id='/down'),
pose=Pose2D(
x=center[0],
y=center[1],
theta=radius
),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True
)
return resp
def request_buoy(self, srv):
print 'requesting', srv
timestamp = self.last_image_time
response = self.find_single_buoy(np.copy(self.last_image), timestamp, srv.target_name)
if response is False:
print 'did not find'
resp = VisionRequest2DResponse(
header=sub8_ros_tools.make_header(frame='/stereo_front/left'),
found=False
)
else:
# Fill in
center, radius = response
resp = VisionRequest2DResponse(
header=Header(stamp=timestamp, frame_id='/stereo_front/left'),
pose=Pose2D(
x=center[0],
y=center[1],
),
max_x=self.last_image.shape[0],
max_y=self.last_image.shape[1],
camera_info=self.image_sub.camera_info,
found=True
)
return resp
def moveto_action(self, position, orientation):
self.client.cancel_goal()
rospy.logwarn("Going to waypoint")
rospy.logwarn("Found server")
# Stay under the water
if position[2] > 0.3:
rospy.logwarn("Waypoint set too high!")
position[2] = -0.5
goal = uf_common_msgs.MoveToGoal(
header=sub8_utils.make_header('/map'),
posetwist=uf_common_msgs.PoseTwist(
pose=geom_msgs.Pose(
position=sub8_utils.numpy_to_point(position),
orientation=sub8_utils.numpy_to_quaternion(orientation)
)
),
speed=0.2,
linear_tolerance=0.1,
angular_tolerance=0.1
)
self.client.send_goal(goal)
# self.client.wait_for_result()
rospy.logwarn("Go to waypoint")
def publish_pose(self):
'''TODO:
Publish velocity in body frame
'''
linear_vel = self.body.getRelPointVel((0.0, 0.0, 0.0))
# TODO: Not 100% on this transpose
angular_vel = self.orientation.transpose().dot(self.angular_vel)
quaternion = self.body.getQuaternion()
translation = self.body.getPosition()
header = sub8_utils.make_header(frame='/world')
pose = geometry.Pose(
position=geometry.Point(*translation),
orientation=geometry.Quaternion(-quaternion[1], -quaternion[2], -quaternion[3], quaternion[0]),
)
twist = geometry.Twist(
linear=geometry.Vector3(*linear_vel),
angular=geometry.Vector3(*angular_vel)
)
odom_msg = Odometry(
header=header,
child_frame_id='/body',
pose=geometry.PoseWithCovariance(
pose=pose
),
twist=geometry.TwistWithCovariance(
twist=twist
)
)
self.truth_odom_pub.publish(odom_msg)
def publish_target_pose(self, position, orientation):
self.target_pose_pub.publish(
PoseStamped(
header=sub8_utils.make_header('/map'),
pose=Pose(
position=sub8_utils.numpy_to_point(position),
orientation=sub8_utils.numpy_to_quaternion(orientation)
)
)
)
self.distance_marker.header = sub8_utils.make_header('/map')
self.distance_marker.text = 'XY: ' + str(self.target_distance) + 'm\n' +\
'Z: ' + str(self.target_depth) + 'm'
self.distance_marker.pose = Pose(
position=sub8_utils.numpy_to_point(position),
orientation=sub8_utils.numpy_to_quaternion(orientation)
)
self.target_distance_pub.publish(self.distance_marker)
def send_wrench(self, force, torque):
'''
Specify wrench in Newtons, Newton-meters
(Are you supposed to say Newtons-meter? Newtons-Meters?)
'''
wrench_msg = geometry_msgs.WrenchStamped(
header=sub8_utils.make_header(),
wrench=geometry_msgs.Wrench(force=geometry_msgs.Vector3(*force),
torque=geometry_msgs.Vector3(*torque)))
self.wrench_pub.publish(wrench_msg)
def publish_target_pose(self, position, orientation):
self.target_pose_pub.publish(
PoseStamped(
header=sub8_utils.make_header('/map'),
pose=Pose(
position=sub8_utils.numpy_to_point(position),
orientation=sub8_utils.numpy_to_quaternion(orientation)
)
)
)
def send_wrench(self, force, torque):
'''
Specify wrench in Newtons, Newton-meters
(Are you supposed to say Newtons-meter? Newtons-Meters?)
'''
wrench_msg = geometry_msgs.WrenchStamped(
header=sub8_utils.make_header(),
wrench=geometry_msgs.Wrench(
force=geometry_msgs.Vector3(*force),
torque=geometry_msgs.Vector3(*torque)
)
)
self.wrench_pub.publish(wrench_msg)
def publish_odom(self, *args):
if self.last_odom is None or self.position_offset is None:
return
msg = self.last_odom
if self.target in msg.name:
header = sub8_utils.make_header(frame='/map')
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
# Add position offset to make the start position (0, 0, -depth)
position_np, orientation_np = sub8_utils.pose_to_numpy(
msg.pose[target_index])
pose = sub8_utils.numpy_quat_pair_to_pose(
position_np - self.position_offset, orientation_np)
self.state_pub.publish(header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
header = sub8_utils.make_header(frame='/world')
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.world_state_pub.publish(
header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
dist = np.linalg.norm(
sub8_utils.twist_to_numpy(twist)) * self.odom_freq
self.pedometry += dist
else:
# fail
return
def publish_odom(self, *args):
if self.last_odom is None or self.position_offset is None:
return
msg = self.last_odom
if self.target in msg.name:
header = sub8_utils.make_header(frame="/map")
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
# Add position offset to make the start position (0, 0, -depth)
position_np, orientation_np = sub8_utils.pose_to_numpy(msg.pose[target_index])
pose = sub8_utils.numpy_quat_pair_to_pose(position_np - self.position_offset, orientation_np)
self.state_pub.publish(
header=header,
child_frame_id="/base_link",
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist),
)
header = sub8_utils.make_header(frame="/world")
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.world_state_pub.publish(
header=header,
child_frame_id="/base_link",
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist),
)
dist = np.linalg.norm(sub8_utils.twist_to_numpy(twist)) * self.odom_freq
self.pedometry += dist
else:
# fail
return
def make_ray(self, base, direction, length, color, frame='/base_link', _id=100, timestamp=None,**kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
marker = visualization_msgs.Marker(
ns='sub',
id=_id,
header=sub8_utils.make_header(frame=frame, stamp=timestamp),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(*color),
scale=Vector3(0.05, 0.05, 0.05),
points=map(lambda o: Point(*o), [base, direction * length]),
lifetime=rospy.Duration(),
**kwargs
)
return marker
def clear_alarm(self):
alarm_contents = {
'alarm_name': self._alarm_name,
'node_name': self._node_name,
'severity': self._severity,
'action_required': False,
'clear': True,
}
if not self.different(**alarm_contents):
return
else:
self._previous_stuff = alarm_contents
alarm_msg = Alarm(header=sub8_ros_tools.make_header(),
**alarm_contents)
self._alarm_pub.publish(alarm_msg)
def set_target(self, pos, orientation=(0.0, 0.0, 0.0, 1.0)):
'''TODO:
Add linear/angular velocity
Publish a trajectory
'''
# self.target = make_pose_stamped(position=pos, orientation=orientation)
target_msg = Trajectory(
header=sub8_utils.make_header(frame='/body'),
trajectory=[Waypoint(
pose=geometry_msgs.Pose(
position=geometry_msgs.Vector3(*pos),
orientation=geometry_msgs.Quaternion(*orientation)
)
)]
)
self.target_pub.publish(target_msg)
def moveto_action(self, position, orientation):
self.client.cancel_goal()
rospy.logwarn("going to waypoint")
rospy.logwarn("Found server")
goal = uf_common_msgs.MoveToGoal(
header=sub8_utils.make_header('/map'),
posetwist=uf_common_msgs.PoseTwist(pose=geom_msgs.Pose(
position=sub8_utils.numpy_to_point(position),
orientation=sub8_utils.numpy_to_quaternion(orientation))),
speed=0.2,
linear_tolerance=0.1,
angular_tolerance=0.1)
self.client.send_goal(goal)
# self.client.wait_for_result()
rospy.logwarn("Got to waypoint")
def raise_alarm(self,
problem_description=None,
parameters=None,
severity=None):
'''Arguments are override parameters'''
got_problem_description = (problem_description != "") or (
self._problem_description is not None)
assert got_problem_description, "No problem description has been set for this alarm"
# Allow overrideable severity
if severity is None:
severity = self._severity
if parameters is not None:
assert isinstance(
parameters,
dict), "Parameters must be in the form of dictionary"
if problem_description is not None:
_problem_description = problem_description
else:
_problem_description = self._problem_description
if parameters is not None:
_parameters = parameters
else:
_parameters = self._parameters
alarm_contents = {
'action_required': self._action_required,
'problem_description': _problem_description,
'parameters': json.dumps(_parameters),
'severity': self._severity,
'alarm_name': self._alarm_name,
'node_name': self._node_name,
'clear': False,
}
if not self.different(**alarm_contents):
return
else:
self._previous_stuff = alarm_contents
alarm_msg = Alarm(header=sub8_ros_tools.make_header(),
**alarm_contents)
self._alarm_pub.publish(alarm_msg)
return alarm_msg
def visualize_pose_est(self):
marker = visualization_msgs.Marker(
ns='torpedo_board/pose_est',
id=0,
header=sub8_utils.make_header(frame=self.current_req.frame_id),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(0.0, 1.0, 10, 0.7),
scale=Vector3(0.05, 0.0, 0.0),
lifetime=rospy.Duration())
pts = self.generate_hom_tb_corners_from_cam(self.minimization_result.x)
marker.points.append(Point(x=pts[0, 0], y=pts[1, 0], z=pts[2, 0]))
marker.points.append(Point(x=pts[0, 1], y=pts[1, 1], z=pts[2, 1]))
marker.points.append(Point(x=pts[0, 2], y=pts[1, 2], z=pts[2, 2]))
marker.points.append(Point(x=pts[0, 3], y=pts[1, 3], z=pts[2, 3]))
marker.points.append(Point(x=pts[0, 0], y=pts[1, 0], z=pts[2, 0]))
self.marker_pub.publish(marker)
def publish_odom(self, *args):
if self.last_odom is None:
return
msg = self.last_odom
if self.target in msg.name:
header = sub8_utils.make_header(frame='/enu')
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.state_pub.publish(header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(pose=pose),
twist=TwistWithCovariance(twist=twist))
def draw_sphere(self, position, color, scaling=(0.11, 0.11, 0.11), _id=4, frame='/stereo_front'):
pose = Pose(
position=sub8_utils.numpy_to_point(position),
orientation=sub8_utils.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='sub',
id=_id,
header=sub8_utils.make_header(frame=frame),
type=visualization_msgs.Marker.SPHERE,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(*scaling),
lifetime=rospy.Duration(),
)
self.rviz_pub.publish(marker)
def clear_alarm(self):
alarm_contents = {
'alarm_name': self._alarm_name,
'node_name': self._node_name,
'severity': self._severity,
'action_required': False,
'clear': True,
}
if not self.different(**alarm_contents):
return
else:
self._previous_stuff = alarm_contents
alarm_msg = Alarm(
header=sub8_ros_tools.make_header(),
**alarm_contents
)
self._alarm_pub.publish(alarm_msg)
def visualize_pose_est(self):
marker = visualization_msgs.Marker(
ns='torpedo_board/pose_est',
id=0,
header=sub8_utils.make_header(frame=self.current_req.frame_id),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(0.0, 1.0, 10, 0.7),
scale=Vector3(0.05, 0.0, 0.0),
lifetime=rospy.Duration()
)
pts = self.generate_hom_tb_corners_from_cam(self.minimization_result.x)
marker.points.append(Point(x=pts[0, 0], y=pts[1, 0], z=pts[2, 0]))
marker.points.append(Point(x=pts[0, 1], y=pts[1, 1], z=pts[2, 1]))
marker.points.append(Point(x=pts[0, 2], y=pts[1, 2], z=pts[2, 2]))
marker.points.append(Point(x=pts[0, 3], y=pts[1, 3], z=pts[2, 3]))
marker.points.append(Point(x=pts[0, 0], y=pts[1, 0], z=pts[2, 0]))
self.marker_pub.publish(marker)
def raise_alarm(self, problem_description=None, parameters=None, severity=None):
'''Arguments are override parameters'''
got_problem_description = (problem_description != "") or (self._problem_description is not None)
assert got_problem_description, "No problem description has been set for this alarm"
# Allow overrideable severity
if severity is None:
severity = self._severity
if parameters is not None:
assert isinstance(parameters, dict), "Parameters must be in the form of dictionary"
if problem_description is not None:
_problem_description = problem_description
else:
_problem_description = self._problem_description
if parameters is not None:
_parameters = parameters
else:
_parameters = self._parameters
alarm_contents = {
'action_required': self._action_required,
'problem_description': _problem_description,
'parameters': json.dumps(_parameters),
'severity': self._severity,
'alarm_name': self._alarm_name,
'node_name': self._node_name,
'clear': False,
}
if not self.different(**alarm_contents):
return
else:
self._previous_stuff = alarm_contents
alarm_msg = Alarm(
header=sub8_ros_tools.make_header(),
**alarm_contents
)
self._alarm_pub.publish(alarm_msg)
return alarm_msg
def depth_callback(self, msg):
'''Handle depth data sent from depth sensor'''
frame = '/depth'
distance = msg.data
marker = self.make_cylinder_marker(
np.array([0.0, 0.0, 0.0]), # place at origin
length=distance,
color=(0.0, 1.0, 0.2, 0.7), # green,
frame=frame,
id=0 # Keep these guys from overwriting eachother
)
self.surface_marker.ns='sub'
self.surface_marker.header = sub8_utils.make_header(frame='/depth')
self.surface_marker.pose = marker.pose
self.surface_marker.text = str(round(distance, 3)) + 'm'
self.surface_marker.id = 0
self.rviz_pub.publish(marker)
self.rviz_pub_t.publish(self.depth_marker)
def request_pipe(self, data):
if self.last_image is None:
return False # Fail if we have no images cached
pose = self.find_pipe(self.last_image)
if (pose is not None):
self.pose_pub.publish(Pose(*pose))
resp = VisionRequestResponse(
pose=PoseStamped(
pose=Pose(*pose),
header=sub8_ros_tools.make_header(frame='/down_camera')
),
found=True
)
return resp
# Indicate a failure to ROS
else:
return False
def make_ray(self,
base,
direction,
length,
color,
frame='/base_link',
**kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
marker = visualization_msgs.Marker(
ns='sub',
id=color.index(0) + 1,
header=sub8_utils.make_header(frame=frame),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(*color),
scale=Vector3(0.05, 0.0, 0.0),
points=map(lambda o: Point(*o), [base, direction * length]),
lifetime=rospy.Duration(),
**kwargs)
return marker
def read_datagram(self):
msg = self.serial.read(
self._datagram_lengths[self.datagram_identifier]
)
start = 0
# gyr
gyro = np.fromstring(
b'\x00' + msg[start + 0:start + 3][::-1] +
b'\x00' + msg[start + 3:start + 6][::-1] +
b'\x00' + msg[start + 6:start + 9][::-1],
dtype='<i'
).astype(np.float32) / (2 ** 14)
self.last_msg = gyro
start += 10
# acc
linear_acceleration = np.fromstring(
b'\x00' + msg[start + 0:start + 3][::-1] +
b'\x00' + msg[start + 3:start + 6][::-1] +
b'\x00' + msg[start + 6:start + 9][::-1],
dtype='<i'
).astype(np.float32) / (2 ** 19)
start += 10
# inc
inclination = np.fromstring(
b'\x00' + msg[start + 0:start + 3][::-1] +
b'\x00' + msg[start + 3:start + 6][::-1] +
b'\x00' + msg[start + 6:start + 9][::-1],
dtype='<i'
).astype(np.float32) / (2 ** 22)
imu_msg = Imu(
header=sub8_ros_tools.make_header(),
angular_velocity=Vector3(*gyro),
linear_acceleration=Vector3(*linear_acceleration)
)
self.imu_pub.publish(imu_msg)
def publish_imu(self, dt):
"""Publishes imu sensor information - orientation, angular velocity, and linear acceleration"""
orientation_matrix = self.pose[:3, :3]
sigma = 0.01
noise = np.random.normal(0.0, sigma, 3)
# Work on a better way to get this
# We can't get this nicely from body.getForce()
g = self.body.vectorFromWorld(self.g)
# Get current velocity of IMU in body frame
cur_vel = np.array(self.body.vectorFromWorld(self.body.getRelPointVel(self.imu_position)))
linear_acc = orientation_matrix.dot(cur_vel - self.last_vel) / dt
linear_acc += g + (noise * dt)
# TODO: Fix frame
angular_vel = orientation_matrix.dot(self.body.getAngularVel()) + (noise * dt)
header = sub8_utils.make_header(frame='/body')
linear = geometry.Vector3(*linear_acc)
angular = geometry.Vector3(*angular_vel)
covariance = [sigma ** 2, 0., 0.,
0., sigma ** 2, 0.,
0., 0., sigma ** 2]
orientation_covariance = [-1, 0., 0.,
0., 0., 0.,
0., 0., 0.]
imu_msg = Imu(
header=header,
angular_velocity=angular,
angular_velocity_covariance=covariance,
linear_acceleration=linear,
linear_acceleration_covariance=covariance,
orientation_covariance=orientation_covariance
)
self.imu_sensor_pub.publish(imu_msg)
def visualize_pose_est(self):
pts = self.generate_hom_tb_corners_from_cam(self.prelim_pose_est)
p1 = Point(x=pts[0, 0], y=pts[1, 0], z=pts[2, 0])
p2 = Point(x=pts[0, 1], y=pts[1, 1], z=pts[2, 1])
p3 = Point(x=pts[0, 2], y=pts[1, 2], z=pts[2, 2])
p4 = Point(x=pts[0, 3], y=pts[1, 3], z=pts[2, 3])
marker = visualization_msgs.Marker(
ns='torpedo_board/pose_est',
id=0,
header=sub8_utils.make_header(frame=self.pose_req_frame),
type=visualization_msgs.Marker.LINE_STRIP,
action=visualization_msgs.Marker.ADD,
color=ColorRGBA(0.0, 1.0, 10, 0.7),
scale=Vector3(0.05, 0.0, 0.0),
lifetime=rospy.Duration()
)
marker.points.append(p1)
marker.points.append(p2)
marker.points.append(p3)
marker.points.append(p4)
marker.points.append(p1)
self.marker_pub.publish(marker)
def make_cylinder_marker(self, base, length, color, frame='/base_link', up_vector=np.array([0.0, 0.0, 1.0]), **kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(
position=sub8_utils.numpy_to_point(center),
orientation=sub8_utils.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='sub',
header=sub8_utils.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs
)
return marker
def draw_sphere(self,
position,
color,
scaling=(0.11, 0.11, 0.11),
_id=4,
frame='/stereo_front'):
pose = Pose(position=sub8_utils.numpy_to_point(position),
orientation=sub8_utils.numpy_to_quaternion(
[0.0, 0.0, 0.0, 1.0]))
marker = visualization_msgs.Marker(
ns='sub',
id=_id,
header=sub8_utils.make_header(frame=frame),
type=visualization_msgs.Marker.SPHERE,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(*scaling),
lifetime=rospy.Duration(),
)
self.rviz_pub.publish(marker)
def publish_dvl(self):
"""Publishes dvl sensor data - twist message, and array of 4 dvl velocities based off of ray orientations"""
correlation = -1
header = sub8_utils.make_header(frame='/body')
vel_dvl_body = self.body.vectorFromWorld(self.body.getRelPointVel(self.dvl_position))
velocity_measurements = []
for ray in self.dvl_ray_orientations:
velocity_measurements.append(VelocityMeasurement(
direction=geometry.Vector3(*ray),
velocity=np.dot(ray, vel_dvl_body),
correlation=correlation
))
dvl_msg = VelocityMeasurements(
header=header,
velocity_measurements=velocity_measurements
)
self.dvl_sensor_pub.publish(dvl_msg)
def publish_odom(self, *args):
if self.last_odom is None:
return
msg = self.last_odom
if self.target in msg.name:
header = sub8_utils.make_header(frame='/enu')
target_index = msg.name.index(self.target)
twist = msg.twist[target_index]
twist = msg.twist[target_index]
pose = msg.pose[target_index]
self.state_pub.publish(
header=header,
child_frame_id='/base_link',
pose=PoseWithCovariance(
pose=pose
),
twist=TwistWithCovariance(
twist=twist
)
)
def as_MoveToGoal(self, linear=[0, 0, 0], angular=[0, 0, 0], **kwargs):
return MoveToGoal(header=make_header(self.frame_id),
posetwist=self.as_PoseTwist(linear, angular),
**kwargs)
def as_MoveToGoal(self, linear=[0, 0, 0], angular=[0, 0, 0], **kwargs):
return MoveToGoal(
header=make_header(self.frame_id),
posetwist=self.as_PoseTwist(linear, angular),
**kwargs
)
def publish_height(self, msg):
'''Sim DVL uses laserscan message to relay height'''
self.dvl_pub.publish(
Float64Stamped(header=sub8_utils.make_header(),
data=float(np.mean(msg.ranges))))
def publish_height(self, msg):
"""Sim DVL uses laserscan message to relay height"""
self.dvl_pub.publish(Float64Stamped(header=sub8_utils.make_header(), data=float(np.mean(msg.ranges))))
