Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
    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")
Ejemplo n.º 3
0
 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))))
Ejemplo n.º 4
0
 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)
Ejemplo n.º 5
0
 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)
             )
         )
     )
Ejemplo n.º 6
0
    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")
Ejemplo n.º 7
0
    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)
Ejemplo n.º 8
0
    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
Ejemplo n.º 9
0
    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)