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 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_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 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 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 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 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 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)
