def __init__(self):
        """
        init method
        """
        self.odometry = Odometry()
        self.odometry_broadcaster = TransformBroadcaster()
        self.odometry_transform = TransformStamped()

        self.x = 0.0
        self.y = 0.0
        self.th = 0.0

        self.vx = 0
        self.vy = 0
        self.vth = 0
        self.seq = 0

        self.dt = 0
        self.delta_x = 0
        self.delta_y = 0
        self.delta_th = 0

        self.odometry_quaternion = trans.quaternion_from_euler(0, 0, self.th)

        self.current_time = rospy.get_time()
        self.last_time = rospy.get_time()
Пример #2
0
    def __init__(self,
                 linear_covariance=0.1,
                 angular_covariance=0.5,
                 tf_frame='odom',
                 camera_fov=1.0):
        """
        :param linear_covariance: (meters) the linear covariance associated with a tag detection
        :param angular_covariance: (radians) the angular covariance of a tag
        :param tf_frame (str): The TF frame of the map / world.
        :param camera_fov (float): The FOV radius of the camera, in meters.
        """
        self.tf = TransformListener()
        self.pub = rospy.Publisher('visible_roombas', RoombaList, queue_size=0)

        self.tf_pub = TransformBroadcaster()

        cov = [0] * 36
        cov[0] = cov[7] = cov[14] = linear_covariance
        cov[21] = cov[28] = cov[35] = angular_covariance
        self.covariance = cov

        self.fov = camera_fov

        self.map_frame = tf_frame

        # Negates x and y coordinates of tag detections
        self.apply_apriltag_fix = True

        rospy.Subscriber('tag_detections', AprilTagDetectionArray,
                         self.on_tags)
Пример #3
0
    def __init__(self, device_id_gaze, model_files):
        super(GazeEstimatorROS, self).__init__(device_id_gaze, model_files)
        self.bridge = CvBridge()
        self.subjects_bridge = SubjectListBridge()

        self.tf_broadcaster = TransformBroadcaster()
        self.tf_listener = TransformListener()

        self.tf_prefix = rospy.get_param("~tf_prefix", "gaze")
        self.headpose_frame = self.tf_prefix + "/head_pose_estimated"
        self.ros_tf_frame = rospy.get_param("~ros_tf_frame",
                                            "/kinect2_ros_frame")

        self.image_subscriber = rospy.Subscriber("/subjects/images",
                                                 MSG_SubjectImagesList,
                                                 self.image_callback,
                                                 queue_size=3,
                                                 buff_size=2**24)
        self.subjects_gaze_img = rospy.Publisher("/subjects/gazeimages",
                                                 Image,
                                                 queue_size=3)

        self.visualise_eyepose = rospy.get_param("~visualise_eyepose",
                                                 default=True)

        self._last_time = rospy.Time().now()
Пример #4
0
def handle_model_state(msg):
    global last_time
    br = TransformBroadcaster()

    model_names = msg.name
    # rospy.logerr_throttle(1,f'{str(model_names)}')

    try:
        # idx = model_names.index(tfPrefix)
        time = rospy.Time.now()
        if time == last_time:
            return
        
        idx = -1

        for i in range(len(model_names)):
            # rospy.logerr_throttle_identical(1,f'{tfPrefix}:{model_names[i]}')
            if tfPrefix == model_names[i]:
                idx = i
                # rospy.logwarn('FOUND')
                break

        pose = msg.pose[idx]
    
        br.sendTransform((pose.position.x, pose.position.y, pose.position.z),
                     (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w),
                     time,
                     tfPrefix + "/" + "base_link",
                     "world")

        last_time = time
    except IndexError:
        rospy.logwarn_throttle(10, f'Cannot find Gazebo model state {tfPrefix}')
Пример #5
0
 def __init__(self):
     self.bfp = True
     self.robot = robot_interface.Robot_Interface()
     self.url = 'http://172.31.76.30:80/ThinkingQ/'
     self.joint_names = ["shoulder_pan_joint",
                    "shoulder_lift_joint", "upperarm_roll_joint",
                    "elbow_flex_joint", "forearm_roll_joint",
                    "wrist_flex_joint", "wrist_roll_joint"]
     self.tf_listener = TransformListener()
     trfm = Transformer()
     self.r2b = trfm.transform_matrix_of_frames(
         'head_camera_rgb_optical_frame', 'base_link')
     self.model = load_model("./model/0.988.h5")
     self.br = TransformBroadcaster()
     self.move_group = MoveGroupInterface("arm", "base_link")
     # self.pose_group = moveit_commander.MoveGroupCommander("arm")
     self.cam = camera.RGBD()
     self.position_cloud = None
     while True:
         try:
             cam_info = self.cam.read_info_data()
             if cam_info is not None:
                 break
         except:
             rospy.logerr('camera info not recieved')
     self.pcm = PCM()
     self.pcm.fromCameraInfo(cam_info)
    def __init__(self, use_dummy_transform=False):
        rospy.init_node('star_center_positioning_node')
        if use_dummy_transform:
            self.odom_frame_name = "odom_dummy"
        else:
            self.odom_frame_name = "odom"

        self.marker_locators = {}
        self.add_marker_locator(
            MarkerLocator(0, (-6 * 12 * 2.54 / 100.0, 0), 0))
        self.add_marker_locator(MarkerLocator(1, (0.0, 0.0), pi))
        self.add_marker_locator(
            MarkerLocator(2, (0.0, 10 * 12 * 2.54 / 100.0), 0))
        self.add_marker_locator(
            MarkerLocator(3, (-6 * 12 * 2.54 / 100.0, 6 * 12 * 2.54 / 100.0),
                          0))

        self.pose_correction = rospy.get_param('~pose_correction', 0.0)

        self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers,
                                           self.process_markers)
        self.odom_sub = rospy.Subscriber("/odom",
                                         Odometry,
                                         self.process_odom,
                                         queue_size=10)
        self.star_pose_pub = rospy.Publisher("STAR_pose",
                                             PoseStamped,
                                             queue_size=10)
        self.continuous_pose = rospy.Publisher("STAR_pose_continuous",
                                               PoseStamped,
                                               queue_size=10)
        # self.star_pose_euler_angle_pub = rospy.Publisher("STAR_pose_euler_angle",Vector3,queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()
  def __init__(self, name='robot_00', x=5.0, y=5.0, theta=0.0):
    rospy.init_node('turtlesim_to_odom')
    self.name = name

    rospy.wait_for_service('/kill')
    kill = rospy.ServiceProxy('/kill',Kill)
    
    if name == 'robot_00':
      try:
        kill('turtle1')
      except:
        pass

    try:
      kill(name)
    except:
      pass

    x = float(x)
    y = float(y)
    theta = float(theta)

    rospy.wait_for_service('/spawn')
    spawn = rospy.ServiceProxy('/spawn',Spawn)
    try:
      spawn(x, y, theta, name)
    except:
      pass

    rospy.Subscriber('pose', TurtlePose, self.pose_callback, queue_size=1)
    self.odom_pub = rospy.Publisher('odometry', Odometry, queue_size=1)
    self.tfb = TransformBroadcaster()
Пример #8
0
    def __init__(self,
                 continuous_estimation=False,
                 joint_states_topic="/joint_states",
                 visual_tags_enabled=True):
        super(Estimation, self).__init__(context="estimation")

        self.locked_joints = []

        self.tf_pub = TransformBroadcaster()
        self.tf_root = "world"

        self.mutex = Lock()

        self.robot_name = rospy.get_param("~robot_name", "")

        self.last_stamp_is_ready = False
        self.last_stamp = rospy.Time.now()
        self.last_visual_tag_constraints = list()

        self.current_stamp = rospy.Time.now()
        self.current_visual_tag_constraints = list()
        self.visual_tags_enabled = visual_tags_enabled

        self.continuous_estimation(SetBoolRequest(continuous_estimation))
        self.estimation_rate = 50  # Hz

        self.subscribers = ros_tools.createSubscribers(self, "/agimus",
                                                       self.subscribersDict)
        self.publishers = ros_tools.createPublishers("/agimus",
                                                     self.publishersDict)
        self.services = ros_tools.createServices(self, "/agimus",
                                                 self.servicesDict)
        self.joint_state_subs = rospy.Subscriber(joint_states_topic,
                                                 JointState,
                                                 self.get_joint_state)
Пример #9
0
    def __init__(self, img_proc=None):
        super(LandmarkMethodROS,
              self).__init__(device_id_facedetection=rospy.get_param(
                  "~device_id_facedetection", default="cuda:0"))
        self.subject_tracker = FaceEncodingTracker() if rospy.get_param(
            "~use_face_encoding_tracker",
            default=True) else SequentialTracker()
        self.bridge = CvBridge()
        self.__subject_bridge = SubjectListBridge()

        self.ros_tf_frame = rospy.get_param("~ros_tf_frame",
                                            "/kinect2_nonrotated_link")

        self.tf_broadcaster = TransformBroadcaster()
        self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
        self.visualise_headpose = rospy.get_param("~visualise_headpose",
                                                  default=True)

        self.pose_stabilizers = {}  # Introduce scalar stabilizers for pose.

        try:
            if img_proc is None:
                tqdm.write("Wait for camera message")
                cam_info = rospy.wait_for_message("/camera_info",
                                                  CameraInfo,
                                                  timeout=None)
                self.img_proc = PinholeCameraModel()
                # noinspection PyTypeChecker
                self.img_proc.fromCameraInfo(cam_info)
            else:
                self.img_proc = img_proc

            if np.array_equal(
                    self.img_proc.intrinsicMatrix().A,
                    np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])):
                raise Exception(
                    'Camera matrix is zero-matrix. Did you calibrate'
                    'the camera and linked to the yaml file in the launch file?'
                )
            tqdm.write("Camera message received")
        except rospy.ROSException:
            raise Exception("Could not get camera info")

        # multiple person images publication
        self.subject_pub = rospy.Publisher("/subjects/images",
                                           MSG_SubjectImagesList,
                                           queue_size=3)
        # multiple person faces publication for visualisation
        self.subject_faces_pub = rospy.Publisher("/subjects/faces",
                                                 Image,
                                                 queue_size=3)

        Server(ModelSizeConfig, self._dyn_reconfig_callback)

        # have the subscriber the last thing that's run to avoid conflicts
        self.color_sub = rospy.Subscriber("/image",
                                          Image,
                                          self.process_image,
                                          buff_size=2**24,
                                          queue_size=3)
Пример #10
0
    def __init__(self):
        rospy.init_node('left_utilitiy_frame_source')

        self.updater = rospy.Service('l_utility_frame_update', FrameUpdate,
                                     self.update_frame)

        self.tfb = TransformBroadcaster()
Пример #11
0
def main():
    rospy.init_node('Odometria_Xbox')

    b = TransformBroadcaster()

    translation = (0.0, 0.0, 0.0)
    rotation = (0.0, 0.0, 0.0, 1.0)  #quaternion
    rate = rospy.Rate(5)  # 5hz

    yant = 0.0
    xant = 0.0

    while not rospy.is_shutdown():
        #y = math.degrees(math.asin(joy.leftX())) #+ yant
        y = joy.leftX() + yant
        x = joy.leftY() + xant

        translation = (x, 0.0, 0.0)
        rotation = tf.transformations.quaternion_from_euler(0, 0, y)

        yant = y
        xant = x

        #print(y)

        b.sendTransform(translation, rotation, Time.now(), 'camera_link',
                        'odom')
        rate.sleep()
Пример #12
0
    def __init__(self):
        rospy.init_node("learn_transform")

        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

        self.possible_base_link_poses = []
        self.baselink_averages = []

        self.rate = rospy.Rate(20)

        self.markers = { "/PATTERN_1": Pose( [-0.56297, 0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_2": Pose( [-0.45097, 0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_3": Pose( [-0.34097, 0.0, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_4": Pose( [-0.34097, -0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_5": Pose( [-0.45097, -0.11, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] ),
                         "/PATTERN_6": Pose( [-0.56297, 0.0, 0.0035],
                                             [0.0, 0.0, 0.0, 1.0] )
                        }

        self.run()
Пример #13
0
    def __init__(self):
        # Public attributes
        if World.tf_listener is None:
            World.tf_listener = TransformListener()
        self.surface = None

        # Private attributes
        self._lock = threading.Lock()
        self._tf_broadcaster = TransformBroadcaster()
        self._im_server = InteractiveMarkerServer(TOPIC_IM_SERVER)
        rospy.wait_for_service(SERVICE_BB)
        self._bb_service = rospy.ServiceProxy(
            SERVICE_BB, FindClusterBoundingBox)
        self._object_action_client = actionlib.SimpleActionClient(
            ACTION_OBJ_DETECTION, UserCommandAction)
        self._object_action_client.wait_for_server()
        rospy.loginfo(
            'Interactive object detection action server has responded.')

        # Setup other ROS machinery
        rospy.Subscriber(
            TOPIC_OBJ_RECOGNITION, GraspableObjectList,
            self.receive_object_info)
        rospy.Subscriber(TOPIC_TABLE_SEG, Marker, self.receive_table_marker)

        # Init
        self.clear_all_objects()
def echo_socket(ws):
    b = TransformBroadcaster()

    f = open("orientation.txt", "a")
    while True:
        message = ws.receive()
        orient_data = message.split(',')
        orient_data = [float(data) for data in orient_data]

        stamped_msg = SensorMsgStamped()
        stamped_msg.data = orient_data
        stamped_msg.header.stamp.secs = Time.now().secs
        stamped_msg.header.stamp.nsecs = Time.now().nsecs

        orient_pub_stamped.publish(stamped_msg)

        ### Publish to Pose topic for visualization ###
        q = quaternion_from_euler(orient_data[1], orient_data[2],
                                  orient_data[3])
        pose_msg = Pose()
        pose_msg.orientation.x = q[0]
        pose_msg.orientation.y = q[1]
        pose_msg.orientation.z = q[2]
        pose_msg.orientation.w = q[3]
        pose_pub.publish(pose_msg)

        b.sendTransform((1, 1, 1), (q[0], q[1], q[2], q[3]), Time.now(),
                        'child_link', 'base_link')
        ### END HERE ###
        print("[INFO:] Orientation{}".format(orient_data))
    ws.send(message)
    print >> f, message

    f.close()
Пример #15
0
    def __init__(self):
        self.listener = tf.TransformListener()

        self.cells = []
        cell_names = rospy.get_param("cells", ["n1", "n2"])

        for cell in cell_names:
            self.cells.append(
                ArtCellCalibration(cell, '/art/' + cell + '/ar_pose_marker',
                                   '/' + cell + '_kinect2_link',
                                   '/marker_detected',
                                   '/' + cell_names[0] + '_kinect2_link',
                                   '/art/' + cell + '/kinect2/qhd/points',
                                   self.listener))

        # self.cells.append(ArtRobotCalibration('pr2', '/pr2/ar_pose_marker',
        #                                      '/odom_combined', '/marker_detected',
        #                                      '/pr2/points'))

        self.calibrated_pub = rospy.Publisher('/art/system/calibrated',
                                              Bool,
                                              queue_size=10,
                                              latch=True)
        self.calibrated = Bool()
        self.calibrated.data = False
        self.calibrated_sended = False
        self.calibrated_pub.publish(self.calibrated)
        self.recalibrate_cell_service = rospy.Service(
            "/art/system/calibrate_cell", RecalibrateCell,
            self.recalibrate_cell_cb)

        self.broadcaster = TransformBroadcaster()
Пример #16
0
class Right_Utility_Frame():
  
    frame = 'base_footprint'
    px = py = pz = 0;
    qx = qy = qz = 0;
    qw = 1;

    def __init__(self):
        rospy.init_node('right_utilitiy_frame_source')
        
        self.updater = rospy.Service('r_utility_frame_update', FrameUpdate, self.update_frame)
        
        self.tfb = TransformBroadcaster()

    def update_frame(self, req):
        ps = req.pose
        self.frame = ps.header.frame_id
        self.px = ps.pose.position.x    
        self.py = ps.pose.position.y    
        self.pz = ps.pose.position.z    
        self.qx = ps.pose.orientation.x
        self.qy = ps.pose.orientation.y
        self.qz = ps.pose.orientation.z
        self.qw = ps.pose.orientation.w

        self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "rh_utility_frame", self.frame)
        rsp = Bool()
        rsp.data = True
        return rsp
Пример #17
0
class Right_Utility_Frame():

    frame = 'base_footprint'
    px = py = pz = 0
    qx = qy = qz = 0
    qw = 1

    def __init__(self):
        rospy.init_node('right_utilitiy_frame_source')

        self.updater = rospy.Service('r_utility_frame_update', FrameUpdate,
                                     self.update_frame)

        self.tfb = TransformBroadcaster()

    def update_frame(self, req):
        ps = req.pose
        self.frame = ps.header.frame_id
        self.px = ps.pose.position.x
        self.py = ps.pose.position.y
        self.pz = ps.pose.position.z
        self.qx = ps.pose.orientation.x
        self.qy = ps.pose.orientation.y
        self.qz = ps.pose.orientation.z
        self.qw = ps.pose.orientation.w

        self.tfb.sendTransform(
            (self.px, self.py, self.pz), (self.qx, self.qy, self.qz, self.qw),
            rospy.Time.now(), "rh_utility_frame", self.frame)
        rsp = Bool()
        rsp.data = True
        return rsp
Пример #18
0
	def __init__(self):


		

		self.tl = TransformListener()
		self.br = TransformBroadcaster()
Пример #19
0
    def __init__(self):

        self.vel_pub = rospy.Publisher(
            '/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=0)
        self.odom_pub = rospy.Publisher('/odometry', Odometry, queue_size=10)

        self.tfb = TransformBroadcaster()

        rospy.sleep(0.5)

        startpose = PoseStamped()
        startpose.header.stamp = rospy.Time.now()
        startpose.header.frame_id = 'odom'
        startpose.pose.orientation.w = 1
        self.publish_pose(startpose, TwistWithCovariance())

        rospy.loginfo("Published initial tf")

        self.set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
        self.set_rate = rospy.ServiceProxy('/mavros/set_stream_rate',
                                           StreamRate)
        self.land = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL)
        self.takeoff = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL)

        rospy.loginfo('Services connected')

        self.vel = Twist()
        self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.on_vel)

        self.takeoff_sub = rospy.Subscriber('/takeoff', Empty, self.on_takeoff)
        self.land_sub = rospy.Subscriber('/land', Empty, self.on_land)

        self.last_pose = PoseStamped()
        self.position_sub = rospy.Subscriber('/mavros/global_position/local',
                                             Odometry, self.on_pose)
    def __init__(self, use_dummy_transform=False):
        rospy.init_node('star_center_positioning_node')
        if use_dummy_transform:
            self.odom_frame_name = "odom_dummy"
        else:
            self.odom_frame_name = "odom"

        self.marker_locators = {}
        self.add_marker_locator(MarkerLocator(0, (0.0, 0.0), 0))
        self.add_marker_locator(MarkerLocator(1, (1.4 / 1.1, 2.0 / 1.1), 0))

        self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers,
                                           self.process_markers)
        self.odom_sub = rospy.Subscriber("odom",
                                         Odometry,
                                         self.process_odom,
                                         queue_size=10)
        self.star_pose_pub = rospy.Publisher("STAR_pose",
                                             PoseStamped,
                                             queue_size=10)
        self.continuous_pose = rospy.Publisher("STAR_pose_continuous",
                                               PoseStamped,
                                               queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()
Пример #21
0
class Left_Utility_Frame():
  
    frame = 'base_footprint'
    px = py = pz = 0;
    qx = qy = qz = 0;
    qw = 1;

    def __init__(self):
        rospy.init_node('left_utilitiy_frame_source')
        
        self.updater = rospy.Service('l_utility_frame_update', FrameUpdate, self.update_frame)
        
        self.tfb = TransformBroadcaster()

    def update_frame(self, req):
        ps = req.pose
        if not (math.isnan(ps.pose.orientation.x) or 
                math.isnan(ps.pose.orientation.y) or
                math.isnan(ps.pose.orientation.z) or
                math.isnan(ps.pose.orientation.w)):
            self.frame = ps.header.frame_id
            self.px = ps.pose.position.x    
            self.py = ps.pose.position.y    
            self.pz = ps.pose.position.z    
            self.qx = ps.pose.orientation.x
            self.qy = ps.pose.orientation.y
            self.qz = ps.pose.orientation.z
            self.qw = ps.pose.orientation.w
        else:
            rospy.logerr("NAN's sent to l_utility_frame_source")

        self.tfb.sendTransform((self.px,self.py,self.pz),(self.qx,self.qy,self.qz,self.qw), rospy.Time.now(), "lh_utility_frame", self.frame)
        rsp = Bool()
        rsp.data = True
        return rsp
Пример #22
0
    def __init__(self):
        self.initialized = False  # make sure we don't perform updates before everything is setup
        rospy.init_node(
            'pf')  # tell roscore that we are creating a new node named "pf"

        self.base_frame = "base_link"  # the frame of the robot base
        self.map_frame = "map"  # the name of the map coordinate frame
        self.odom_frame = "odom"  # the name of the odometry coordinate frame
        self.scan_topic = "scan"  # the topic where we will get laser scans from

        self.n_particles = 500  # the number of particles to use

        self.d_thresh = 0.1  # the amount of linear movement before performing an update
        self.a_thresh = math.pi / 12  # the amount of angular movement before performing an update

        self.laser_max_distance = 2.0  # maximum penalty to assess in the likelihood field model

        # Setup pubs and subs

        # pose_listener responds to selection of a new approximate robot location (for instance using rviz)
        rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
                         self.update_initial_pose)

        # publish the current particle cloud.  This enables viewing particles in rviz.
        self.particle_pub = rospy.Publisher("particlecloud",
                                            PoseArray,
                                            queue_size=10)
        # laser_subscriber listens for data from the lidar
        rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

        # enable listening for and broadcasting coordinate transforms
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

        self.particle_cloud = []

        # change use_projected_stable_scan to True to use point clouds instead of laser scans
        self.use_projected_stable_scan = False
        self.last_projected_stable_scan = None
        if self.use_projected_stable_scan:
            # subscriber to the odom point cloud
            rospy.Subscriber("projected_stable_scan", PointCloud,
                             self.projected_scan_received)

        self.current_odom_xy_theta = []

        # request the map from the map server
        rospy.wait_for_service('static_map')
        try:
            map_server = rospy.ServiceProxy('static_map', GetMap)
            map = map_server().map
            print map.info.resolution
        except:
            print "Service call failed!"

        # initializes the occupancyfield which contains the map
        self.occupancy_field = OccupancyField(map)
        print "initialized"
        self.initialized = True
Пример #23
0
class OdometryPublisher:
    def __init__(self,
                 frame="odom",
                 child_frame="base_footprint",
                 queue_size=5):
        self._frame_id = frame
        self._child_frame_id = child_frame
        self._publisher = Publisher("odom", Odometry, queue_size=queue_size)
        self._broadcaster = TransformBroadcaster()
        self._odom_msg = Odometry()
        self._odom_msg.header.frame_id = self._frame_id
        self._odom_msg.child_frame_id = self._child_frame_id

    def _msg(self, now, x, y, quat, v, w):
        """
        Updates the Odometry message
        :param now: Current time 
        :param x_dist: Current x distance traveled 
        :param y_dist: Current y distance traveled
        :param quat: Quaternion of the orientation
        :param v: Linear velocity
        :param w: Angular velocity
        :return: Odometry message
        """
        self._odom_msg.header.stamp = now
        self._odom_msg.pose.pose.position.x = x
        self._odom_msg.pose.pose.position.y = y
        self._odom_msg.pose.pose.position.z = 0
        self._odom_msg.pose.pose.orientation = quat
        self._odom_msg.twist.twist = Twist(Vector3(v, 0, 0), Vector3(0, 0, w))

        return self._odom_msg

    def publish(self, now, x, y, v, w, heading, log=False):
        """
        Publishes an Odometry message
        :param cdist: Center (or average) distance of wheel base
        :param v: Linear velocity
        :param w: Angular velocity
        :param heading: Heading (or yaw)
        :param quat: Quaternion of orientation
        :return: None
        """
        quat = Quaternion()
        quat.x = 0.0
        quat.y = 0.0
        quat.z = math.sin(heading / 2)
        quat.w = math.cos(heading / 2)

        self._broadcaster.sendTransform((x, y, 0),
                                        (quat.x, quat.y, quat.z, quat.w), now,
                                        self._child_frame_id, self._frame_id)

        self._publisher.publish(self._msg(now, x, y, quat, v, w))

        if log:
            loginfo(
                "Publishing Odometry: heading {:02.3f}, dist {:02.3f}, velocity {:02.3f}/{:02.3f}"
                .format(heading, math.sqrt(x**2 + y**2), v, w))
Пример #24
0
    def __init__(self):
        rospy.init_node('Base_position', anonymous=True)
        self.base_pub = rospy.Publisher('/odometry/filtered_map',
                                        Odometry,
                                        queue_size=1)

        self.br = TransformBroadcaster()
        trans = (0, 0, 0)
        rot = (0, 0, 0, 1)
        stamp = rospy.Time.now()
        self.br.sendTransform(trans, rot, stamp, "base_link", "map")

        self.Current_goal = []
        self.Movedir = [0, 0, 0]
        self.VPose = Odometry()
        self.VPose.header.stamp = rospy.Time.now()
        self.VPose.header.frame_id = "map"
        self.VPose.pose.pose.position.x = float(0)
        self.VPose.pose.pose.position.y = float(0)
        self.VPose.pose.pose.position.z = float(0)
        self.VPose.pose.pose.orientation.x = float(0)
        self.VPose.pose.pose.orientation.y = float(0)
        self.VPose.pose.pose.orientation.z = float(0)
        self.VPose.pose.pose.orientation.w = float(1)

        rate = rospy.Rate(0.5)  # 10hz

        while not rospy.is_shutdown():
            rospy.Subscriber("/move_base_simple/goal",
                             PoseStamped,
                             self.Find_goal,
                             queue_size=1)
            if self.Current_goal == []:
                self.Movedir = [0, 0, 0]
            else:
                Movex = self.Current_goal.pose.position.x - self.VPose.pose.pose.position.x
                Movey = self.Current_goal.pose.position.y - self.VPose.pose.pose.position.y
                Movez = self.Current_goal.pose.position.z - self.VPose.pose.pose.position.z
                Movemag = math.sqrt((Movex)**2 + (Movey)**2)
                self.Movedir = [Movex, Movey, 0]
                print(self.Movedir)
                #self.Movedir = [1,0,0]
            self.VPose.header.stamp = rospy.Time.now()
            self.VPose.pose.pose.position.x += self.Movedir[0]
            self.VPose.pose.pose.position.y += self.Movedir[1]
            self.VPose.pose.pose.position.z += self.Movedir[2]
            self.br = TransformBroadcaster()
            trans = (self.VPose.pose.pose.position.x,
                     self.VPose.pose.pose.position.y,
                     self.VPose.pose.pose.position.z)
            rot = (self.VPose.pose.pose.orientation.x,
                   self.VPose.pose.pose.orientation.y,
                   self.VPose.pose.pose.orientation.z,
                   self.VPose.pose.pose.orientation.w)
            self.br.sendTransform(trans, rot, self.VPose.header.stamp,
                                  "base_link",
                                  "map")  #Pose.header.stamp,"base","map")
            self.base_pub.publish(self.VPose)
            rate.sleep()
Пример #25
0
 def __init__(self):
     self.tf = TransformListener()
     self.tfb = TransformBroadcaster()
     self.active = True
     self.head_pose = PoseStamped()
     self.goal_pub = rospy.Publisher('goal_pose', PoseStamped)
     rospy.Subscriber('/head_center', PoseStamped, self.head_pose_cb)
     rospy.Service('/point_mirror', PointMirror, self.point_mirror_cb)
Пример #26
0
 def __init__(self):
     rospy.init_node("iidre_uwb_xyz_publisher")
     self.tfb = TransformBroadcaster()
     self.serial = None
     self.device_name = rospy.get_param("name", "uwb")
     self.device_port = rospy.get_param("port", "/dev/ttyACM0")
     self.device_frame_id = rospy.get_param("frame_id", "map")
     self.publish_anchors = rospy.get_param("publish_anchors", True)
Пример #27
0
 def update_transform(self, pose, target_frame='base_laser_link'):
     # Updates the transform between the map frame and the odom frame
     br = TransformBroadcaster()
     br.sendTransform((pose[0], pose[1], 0),
                       quaternion_from_euler(0, 0, pose[2]+math.pi),
                       rospy.get_rostime(),
                       target_frame,
                       'map')
Пример #28
0
    def __init__(self, config):
        rospy.loginfo("Start to intialize moveit_control_interface")

        # Config
        self.cfg = config.cfg

        # TF
        self.tf = TransformListener()
        self.br = TransformBroadcaster()

        rospy.Subscriber(self.cfg['topicMoveGroupResult'],
                         MoveGroupActionResult, self.cb_move_group_result)
        rospy.Subscriber(self.cfg['topicTrajectoryExecutionResult'],
                         ExecuteTrajectoryActionResult,
                         self.cb_trajectory_execution_result)

        # Wait for Moveit action server
        moveit_interface_found = False
        while not moveit_interface_found:
            try:
                self.robot = moveit_commander.RobotCommander()
                self.scene = moveit_commander.PlanningSceneInterface()
                self.group = moveit_commander.MoveGroupCommander(
                    self.cfg['move_group_name'])
                moveit_interface_found = True
            except Exception as e:
                rospy.logerr(e.message)
                moveit_interface_found = False
                rospy.logerr("Retrying to connect to MoveIt action server ...")
                rospy.signal_shutdown('No MoveIt interface found')
                return

        # Create an inverse mapping of the joint names
        self.active_joints = self.group.get_active_joints()
        rospy.loginfo("============ Active joints: %d" %
                      len(self.active_joints))
        self.active_joints_id = {}
        i = 0
        for joint_name in self.active_joints:
            self.active_joints_id[i] = joint_name
            i += 1
        rospy.loginfo(self.active_joints_id)

        # Joint/Goal tolerances for motion planning:
        rospy.loginfo(
            "============ Goal tolerances: joint, position, orientation")
        self.group.set_goal_position_tolerance(
            self.cfg['goal_position_tolerance'])
        self.group.set_goal_joint_tolerance(self.cfg['goal_joint_tolerance'])
        self.group.set_goal_orientation_tolerance(
            self.cfg['goal_orientation_tolerance'])
        rospy.loginfo("Joint-, position-, orientation tolerance: ")
        rospy.loginfo(
            self.group.get_goal_tolerance()
        )  # Return a tuple of goal tolerances: joint, position and orientation.

        rospy.loginfo("============ Reference frame for poses of end-effector")
        rospy.loginfo(self.group.get_pose_reference_frame())
Пример #29
0
    def __init__(self):
        self.image_height = rospy.get_param("~image_height", 36)
        self.image_width = rospy.get_param("~image_width", 60)
        self.bridge = CvBridge()
        self.subjects_bridge = SubjectListBridge()

        self.tf_broadcaster = TransformBroadcaster()
        self.tf_listener = TransformListener()

        self.use_last_headpose = rospy.get_param("~use_last_headpose", True)
        self.tf_prefix = rospy.get_param("~tf_prefix", "gaze")
        self.last_phi_head, self.last_theta_head = None, None

        self.rgb_frame_id_ros = rospy.get_param("~rgb_frame_id_ros",
                                                "/kinect2_nonrotated_link")

        self.headpose_frame = self.tf_prefix + "/head_pose_estimated"
        self.device_id_gazeestimation = rospy.get_param(
            "~device_id_gazeestimation", default="/gpu:0")
        with tensorflow.device(self.device_id_gazeestimation):
            config = tensorflow.ConfigProto(inter_op_parallelism_threads=1,
                                            intra_op_parallelism_threads=1)
            if "gpu" in self.device_id_gazeestimation:
                config.gpu_options.allow_growth = True
                config.gpu_options.per_process_gpu_memory_fraction = 0.3
            config.log_device_placement = False
            self.sess = tensorflow.Session(config=config)
            set_session(self.sess)

        model_files = rospy.get_param("~model_files")

        self.models = []
        for model_file in model_files:
            tqdm.write('Load model ' + model_file)
            model = load_model(os.path.join(
                rospkg.RosPack().get_path('rt_gene'), model_file),
                               custom_objects={
                                   'accuracy_angle': accuracy_angle,
                                   'angle_loss': angle_loss
                               })
            # noinspection PyProtectedMember
            model._make_predict_function(
            )  # have to initialize before threading
            self.models.append(model)
        tqdm.write('Loaded ' + str(len(self.models)) + ' models')

        self.graph = tensorflow.get_default_graph()

        self.image_subscriber = rospy.Subscriber('/subjects/images',
                                                 MSG_SubjectImagesList,
                                                 self.image_callback,
                                                 queue_size=3)
        self.subjects_gaze_img = rospy.Publisher('/subjects/gazeimages',
                                                 Image,
                                                 queue_size=3)

        self.average_weights = np.array([0.1, 0.125, 0.175, 0.2, 0.4])
        self.gaze_buffer_c = {}
Пример #30
0
class LaunchObserver(object):
    """
    Keeps track of the flying/landed state of a single drone, and publishes 
    a tf message keeping track of the coordinates from which the drone most recently launched.
    """
    def __init__(self):
        self.tfl = TransformListener()
        self.tfb = TransformBroadcaster()
        self.flying_state_sub = rospy.Subscriber(
            'states/ardrone3/PilotingState/FlyingStateChanged',
            Ardrone3PilotingStateFlyingStateChanged, self.on_flying_state)

        self.is_flying = True

        self.RATE = 5  # republish hz

        self.saved_translation = [0, 0, 0]  # In meters
        self.saved_yaw = 0  # In radians

        self.name = rospy.get_namespace().strip('/')
        self.base_link = self.name + '/base_link'
        self.launch = self.name + '/launch'
        self.odom = self.name + '/odom'

    def on_flying_state(self, msg):
        self.is_flying = msg.state in [
            Ardrone3PilotingStateFlyingStateChanged.state_takingoff,
            Ardrone3PilotingStateFlyingStateChanged.state_hovering,
            Ardrone3PilotingStateFlyingStateChanged.state_flying,
            Ardrone3PilotingStateFlyingStateChanged.state_landing,
            Ardrone3PilotingStateFlyingStateChanged.state_emergency_landing
        ]

    def spin(self):
        r = rospy.Rate(self.RATE)
        self.tfl.waitForTransform(self.odom, self.base_link, rospy.Time(0),
                                  rospy.Duration.from_sec(99999))
        while not rospy.is_shutdown():
            if not self.is_flying:
                # On the ground, update the transform
                pos, quat = self.tfl.lookupTransform(self.base_link, self.odom,
                                                     rospy.Time(0))

                pos[2] = 0
                self.saved_translation = pos
                _, _, self.saved_yaw = euler_from_quaternion(quat)

            time = max(rospy.Time.now(),
                       self.tfl.getLatestCommonTime(
                           self.odom, self.base_link)) + (
                               2 * rospy.Duration.from_sec(1.0 / self.RATE))

            self.tfb.sendTransform(self.saved_translation,
                                   quaternion_from_euler(0, 0, self.saved_yaw),
                                   time, self.odom, self.launch)

            r.sleep()
Пример #31
0
  def __init__(self):
    self.image_pub = rospy.Publisher("/postImage",Image,queue_size=50)

    self.bridge = CvBridge()
    self.image_sub = rospy.Subscriber("/camera/image_raw",Image,self.callback)

    self.h = Header()
    self.tb = TransformBroadcaster()
    self.img_tf=TransformStamped()
Пример #32
0
	def __init__(self):
		rospy.init_node('republish_tag_tf')
		self.tf = TransformListener()
		self.br = TransformBroadcaster()
		
		rospy.Subscriber("/tag_detections", AprilTagDetectionArray, self.tags_callback, queue_size=10)
		
		self.tag_detections = rospy.Publisher('/selective_grasping/tag_detections', Int16MultiArray, queue_size=10)
		self.detections_list = np.zeros(8)
Пример #33
0
class RiCDiffCloseLoop(Device):

    def __init__(self, param, output):
        Device.__init__(self, param.getCloseDiffName(), output)
        self._baseLink = param.getCloseDiffBaseLink()
        self._odom = param.getCloseDiffOdom()
        self._maxAng = param.getCloseDiffMaxAng()
        self._maxLin = param.getCloseDiffMaxLin()
        self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz())
        self._broadCase = TransformBroadcaster()
        Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1)
        Service('%s/setOdometry' % self._name, set_odom, self.setOdom)

    def diffServiceCallback(self, msg):

        if msg.angular.z > self._maxAng:
            msg.angular.z = self._maxAng
        elif msg.angular.z < -self._maxAng:
            msg.angular.z = -self._maxAng

        if msg.linear.x > self._maxLin:
            msg.linear.x = self._maxLin
        elif msg.linear.x < -self._maxLin:
            msg.linear.x = -self._maxLin
        # print msg.angular.z, msg.linear.x
        self._output.write(CloseDiffRequest(msg.angular.z, msg.linear.x).dataTosend())


    def setOdom(self, req):
        self._output.write(CloseDiffSetOdomRequest(req.x, req.y, req.theta).dataTosend())
        return set_odomResponse(True)

    def publish(self, data):
        q = Quaternion()
        q.x = 0
        q.y = 0
        q.z = data[6]
        q.w = data[7]
        odomMsg = Odometry()
        odomMsg.header.frame_id = self._odom
        odomMsg.header.stamp = rospy.get_rostime()
        odomMsg.child_frame_id = self._baseLink
        odomMsg.pose.pose.position.x = data[0]
        odomMsg.pose.pose.position.y = data[1]
        odomMsg.pose.pose.position.z = 0
        odomMsg.pose.pose.orientation = q
        self._pub.publish(odomMsg)
        traMsg = TransformStamped()
        traMsg.header.frame_id = self._odom
        traMsg.header.stamp = rospy.get_rostime()
        traMsg.child_frame_id = self._baseLink
        traMsg.transform.translation.x = data[0]
        traMsg.transform.translation.y = data[1]
        traMsg.transform.translation.z = 0
        traMsg.transform.rotation = q
        self._broadCase.sendTransformMessage(traMsg)
class OptiTrackClient():
    def __init__(self, addr, port, obj_names, world, dt, rate=120):
        """
        Class tracking optitrack markers through VRPN and publishing their TF frames as well as the transformation
        from the robot's world frame to the optitrack frame
        :param addr: IP of the VRPN server
        :param port: Port of the VRPN server
        :param obj_names: Name of the tracked rigid bodies
        :param world: Name of the robot's world frame (base_link, map, base, ...)
        :param dt: Delta T in seconds whilst a marker is still considered tracked
        :param rate: Rate in Hertz of the publishing loop
        """
        self.rate = rospy.Rate(rate)
        self.obj_names = obj_names
        self.world = world
        self.dt = rospy.Duration(dt)
        self.tfb = TransformBroadcaster()

        self.trackers = []
        for obj in obj_names:
            t = vrpn.receiver.Tracker('{}@{}:{}'.format(obj, addr, port))
            t.register_change_handler(obj, self.handler, 'position')
            self.trackers.append(t)

        self.tracked_objects = {}

    @property
    def recent_tracked_objects(self):
        """ Only returns the objects that have been tracked less than 20ms ago. """
        f = lambda name: (rospy.Time.now() - self.tracked_objects[name].timestamp)
        return dict([(k, v) for k, v in self.tracked_objects.iteritems() if f(k) < self.dt])

    def handler(self, obj, data):
        self.tracked_objects[obj] = TrackedObject(data['position'],
                                                  data['quaternion'],
                                                  rospy.Time.now())

    def run(self):
        while not rospy.is_shutdown():
            for t in self.trackers:
                t.mainloop()

            # Publish the calibration matrix
            calib_matrix = rospy.get_param("/optitrack/calibration_matrix")
            self.tfb.sendTransform(calib_matrix[0], calib_matrix[1], rospy.Time.now(), "optitrack_frame", self.world)

            # Publish all other markers as frames
            for k, v in self.recent_tracked_objects.iteritems():
                self.tfb.sendTransform(v.position, v.quaternion, v.timestamp, k, "optitrack_frame")

            self.rate.sleep()
class Rebroadcaster(object):
    def __init__(self):
        self.broadcaster = TransformBroadcaster()
        self.ell_param_sub = rospy.Subscriber('ellipsoid_params', EllipsoidParams, self.ell_cb)
        self.transform = None

    def ell_cb(self, ell_msg):
        print "Got transform"
        self.transform = copy.copy(ell_msg.e_frame)

    def send_transform(self):
        print "spinning", self.transform
        if self.transform is not None:
            print "Sending frame"
            self.broadcaster.sendTransform(self.transform)
    def __init__(self, use_dummy_transform=False):
        rospy.init_node("star_center_positioning_node")

        self.robot_name = rospy.get_param("~robot_name", "")
        self.odom_frame_name = self.robot_name + "_odom"
        self.base_link_frame_name = self.robot_name + "_base_link"

        self.marker_locators = {}
        self.add_marker_locator(MarkerLocator(0, (-6 * 12 * 2.54 / 100.0, 0), 0))
        self.add_marker_locator(MarkerLocator(1, (0.0, 0.0), pi))
        self.add_marker_locator(MarkerLocator(2, (0.0, 10 * 12 * 2.54 / 100.0), 0))
        self.add_marker_locator(MarkerLocator(3, (-6 * 12 * 2.54 / 100.0, 6 * 12 * 2.54 / 100.0), 0))
        self.add_marker_locator(MarkerLocator(4, (0.0, 6 * 12 * 2.54 / 100), pi))
        self.add_marker_locator(MarkerLocator(5, (-4 * 12 * 2.54 / 100.0, 14 * 12 * 2.54 / 100.0), pi))

        self.pose_correction = rospy.get_param("~pose_correction", 0.0)
        self.phase_offset = rospy.get_param("~phase_offset", 0.0)
        self.is_flipped = rospy.get_param("~is_flipped", False)

        srv = Server(STARPoseConfig, self.config_callback)

        self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers)
        self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10)
        self.star_pose_pub = rospy.Publisher("STAR_pose", PoseStamped, queue_size=10)
        self.continuous_pose = rospy.Publisher("STAR_pose_continuous", PoseStamped, queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()
    def __init__(self, use_dummy_transform=False):
        rospy.init_node('star_center_positioning_node')
        if use_dummy_transform:
            self.odom_frame_name = ROBOT_NAME+"_odom_dummy"
        else:
            self.odom_frame_name = ROBOT_NAME+"_odom"

        self.marker_locators = {}
        self.add_marker_locator(MarkerLocator(0,(-6*12*2.54/100.0,0),0))
        self.add_marker_locator(MarkerLocator(1,(0.0,0.0),pi))
        self.add_marker_locator(MarkerLocator(2,(0.0,10*12*2.54/100.0),0))
        self.add_marker_locator(MarkerLocator(3,(-6*12*2.54/100.0,6*12*2.54/100.0),0))
        self.add_marker_locator(MarkerLocator(4,(0.0,6*12*2.54/100.0),pi))

        self.pose_correction = rospy.get_param('~pose_correction',0.0)
        self.phase_offset = rospy.get_param('~phase_offset',0.0)
        self.is_flipped = rospy.get_param('~is_flipped',False)


        srv = Server(STARPoseConfig, self.config_callback)

        self.marker_sub = rospy.Subscriber("ar_pose_marker",
                                           ARMarkers,
                                           self.process_markers)
        self.odom_sub = rospy.Subscriber(ROBOT_NAME+"/odom", Odometry, self.process_odom, queue_size=10)
        self.star_pose_pub = rospy.Publisher(ROBOT_NAME+"/STAR_pose",PoseStamped,queue_size=10)
        self.continuous_pose = rospy.Publisher(ROBOT_NAME+"/STAR_pose_continuous",PoseStamped,queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()
Пример #38
0
    def __init__(self):
        self.listener = TransformListener()
        self.br = TransformBroadcaster()
        self.pub_freq = 100.0
        self.parent_frame_id = "world"
        self.child1_frame_id = "reference1"
        self.child2_frame_id = "reference2"
        self.child3_frame_id = "reference3"
        self.child4_frame_id = "reference4"
        rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference2)
        rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference3)
        rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference4)
        rospy.sleep(1.0)

        recorder_0 = RecordingManager("all")
        recorder_1 = RecordingManager("test1")
        recorder_2 = RecordingManager("test2")
        recorder_3 = RecordingManager("test3")

        recorder_0.start()
        recorder_1.start()
        self.pub_line(length=1, time=5)
        recorder_1.stop()
        recorder_2.start()
        self.pub_quadrat(length=2, time=10)
        recorder_2.stop()
        recorder_3.start()
        self.pub_circ(radius=2, time=5)
        recorder_3.stop()
        recorder_0.stop()
Пример #39
0
    def __init__(self):

        if World.tf_listener == None:
            World.tf_listener = TransformListener()
        self._lock = threading.Lock()
        self.surface = None
        self._tf_broadcaster = TransformBroadcaster()
        self._im_server = InteractiveMarkerServer('world_objects')
        bb_service_name = 'find_cluster_bounding_box'
        rospy.wait_for_service(bb_service_name)
        self._bb_service = rospy.ServiceProxy(bb_service_name,
                                            FindClusterBoundingBox)
        rospy.Subscriber('interactive_object_recognition_result',
            GraspableObjectList, self.receive_object_info)
        self._object_action_client = actionlib.SimpleActionClient(
            'object_detection_user_command', UserCommandAction)
        self._object_action_client.wait_for_server()
        rospy.loginfo('Interactive object detection action ' +
                      'server has responded.')
        self.clear_all_objects()
        # The following is to get the table information
        rospy.Subscriber('tabletop_segmentation_markers',
                         Marker, self.receive_table_marker)

        rospy.Subscriber("ar_pose_marker",
                         AlvarMarkers, self.receive_ar_markers)
        self.marker_dims = Vector3(0.04, 0.04, 0.01)
    def __init__(self, use_dummy_transform=False):
        print "init"
        rospy.init_node("star_center_positioning_node")
        if use_dummy_transform:
            self.odom_frame_name = ROBOT_NAME + "_odom_dummy"  # identify this odom as ROBOT_NAME's
        else:
            self.odom_frame_name = ROBOT_NAME + "_odom"  # identify this odom as ROBOT_NAME's

        self.marker_locators = {}

        self.add_marker_locator(MarkerLocator(0, (-6 * 12 * 2.54 / 100.0, 0), 0))
        self.add_marker_locator(MarkerLocator(1, (0.0, 0.0), pi))
        self.add_marker_locator(MarkerLocator(2, (0.0, 10 * 12 * 2.54 / 100.0), 0))
        self.add_marker_locator(MarkerLocator(3, (-6 * 12 * 2.54 / 100.0, 6 * 12 * 2.54 / 100.0), 0))

        self.pose_correction = rospy.get_param("~pose_correction", 0.0)

        self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers)
        self.odom_sub = rospy.Subscriber(
            ROBOT_NAME + "/odom", Odometry, self.process_odom, queue_size=10
        )  # publish and subscribe to the correct robot's topics
        self.star_pose_pub = rospy.Publisher(ROBOT_NAME + "/STAR_pose", PoseStamped, queue_size=10)
        self.continuous_pose = rospy.Publisher(ROBOT_NAME + "/STAR_pose_continuous", PoseStamped, queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()
Пример #41
0
	def __init__(self):
		self.initialized = False		# make sure we don't perform updates before everything is setup
		rospy.init_node('pf')			# tell roscore that we are creating a new node named "pf"

		self.base_frame = "base_link"	# the frame of the robot base
		self.map_frame = "map"			# the name of the map coordinate frame
		self.odom_frame = "odom"		# the name of the odometry coordinate frame
		self.scan_topic = "scan"		# the topic where we will get laser scans from 

		self.n_particles = 300			# the number of particles to use

		self.d_thresh = 0.2				# the amount of linear movement before performing an update
		self.a_thresh = math.pi/6		# the amount of angular movement before performing an update

		self.laser_max_distance = 2.0	# maximumls penalty to assess in the likelihood field model

		# TODO: define additional constants if needed

		#### DELETE BEFORE POSTING
		self.alpha1 = 0.2
		self.alpha2 = 0.2
		self.alpha3 = 0.2
		self.alpha4 = 0.2
		self.z_hit = 0.5
		self.z_rand = 0.5
		self.sigma_hit = 0.1
		##### DELETE BEFORE POSTING

		# Setup pubs and subs

		# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
		self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
		# publish the current particle cloud.  This enables viewing particles in rviz.
		self.particle_pub = rospy.Publisher("particlecloud", PoseArray)
		self.a_particle_pub = rospy.Publisher("particle", PoseStamped)

		# laser_subscriber listens for data from the lidar
		self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

		# enable listening for and broadcasting coordinate transforms
		self.tf_listener = TransformListener()
		self.tf_broadcaster = TransformBroadcaster()

		self.particle_cloud = []

		self.current_odom_xy_theta = []

		# request the map
		# Difficulty level 2

		rospy.wait_for_service("static_map")
		static_map = rospy.ServiceProxy("static_map", GetMap)
		try:
			map = static_map().map
		except:
			print "error receiving map"

		self.occupancy_field = OccupancyField(map)
		self.initialized = True
Пример #42
0
 def __init__(self):
     self.tf = TransformListener()
     self.tfb = TransformBroadcaster()
     self.active = True
     self.head_pose = PoseStamped()
     self.goal_pub = rospy.Publisher('goal_pose', PoseStamped)
     rospy.Subscriber('/head_center', PoseStamped, self.head_pose_cb)
     rospy.Service('/point_mirror', PointMirror, self.point_mirror_cb)
 def __init__(self):
     self.tfBroadcaster = TransformBroadcaster()
     q = transformations.quaternion_from_euler(0,0,pi)
     self.humanPose = Pose(Point(5, 0, 1.65), Quaternion(*q))
     self.markerPublisher = rospy.Publisher('visualization_marker', Marker)
     rospy.loginfo('Initialized.')
     self.startTime = time.time()
     rospy.init_node('fake_human_node', anonymous=True)
Пример #44
0
 def __init__(self, E=1, is_oblate=False):
     self.A = 1
     self.E = E
     #self.B = np.sqrt(1. - E**2)
     self.a = self.A * self.E
     self.is_oblate = is_oblate
     self.center = None
     self.frame_broadcaster = TransformBroadcaster()
     self.center_tf_timer = None
Пример #45
0
    def __init__(self):
        self.initialized = False        # make sure we don't perform updates before everything is setup
        rospy.init_node('pf')           # tell roscore that we are creating a new node named "pf"

        self.base_frame = "base_link"   # the frame of the robot base
        self.map_frame = "map"          # the name of the map coordinate frame
        self.odom_frame = "odom"        # the name of the odometry coordinate frame
        self.scan_topic = "scan"        # the topic where we will get laser scans from

        self.n_particles = 100          # the number of particles to use

        self.d_thresh = 0.2             # the amount of linear movement before performing an update
        self.a_thresh = math.pi/6       # the amount of angular movement before performing an update

        self.laser_max_distance = 2.0   # maximum penalty to assess in the likelihood field model

        self.sigma = 0.08 

        # TODO: define additional constants if needed

        # Setup pubs and subs

        # pose_listener responds to selection of a new approximate robot location (for instance using rviz)
        self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)

        
            
        # publish the current particle cloud.  This enables viewing particles in rviz.
        self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
        self.marker_pub = rospy.Publisher("markers", MarkerArray, queue_size=10)

        # laser_subscriber listens for data from the lidar
        # Dados do Laser: Mapa de verossimilhança/Occupancy field/Likehood map e Traçado de raios.
        # Traçado de raios: Leitura em ângulo que devolve distância, através do sensor. Dado o mapa,
        # extender a direção da distância pra achar um obstáculo. 
        self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

        # enable listening for and broadcasting coordinate transforms
        #atualização de posição(odometria)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

        self.particle_cloud = []

        self.current_odom_xy_theta = []

        #Chamar o mapa a partir de funcao externa
        mapa = chama_mapa.obter_mapa()

        # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
        # TODO: fill in the appropriate service call here.  The resultant map should be assigned be passed
        #       into the init method for OccupancyField

        # for now we have commented out the occupancy field initialization until you can successfully fetch the map
        self.occupancy_field = OccupancyField(mapa)
        self.initialized = True
 def __init__(self):
     rospy.init_node('normal_approach_right')
     rospy.Subscriber('norm_approach_right', PoseStamped, self.update_frame)
     rospy.Subscriber('r_hand_pose', PoseStamped, self.update_curr_pose)
     rospy.Subscriber('wt_lin_move_right',Float32, self.linear_move)
     self.goal_out = rospy.Publisher('wt_right_arm_pose_commands', PoseStamped)
     self.move_arm_out = rospy.Publisher('wt_move_right_arm_goals', PoseStamped)
     self.tf = TransformListener()
     self.tfb = TransformBroadcaster()
     self.wt_log_out = rospy.Publisher('wt_log_out', String )
Пример #47
0
 def __init__(self, param, output):
     Device.__init__(self, param.getCloseDiffName(), output)
     self._baseLink = param.getCloseDiffBaseLink()
     self._odom = param.getCloseDiffOdom()
     self._maxAng = param.getCloseDiffMaxAng()
     self._maxLin = param.getCloseDiffMaxLin()
     self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz())
     self._broadCase = TransformBroadcaster()
     Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1)
     Service('%s/setOdometry' % self._name, set_odom, self.setOdom)
Пример #48
0
 def __init__(self):
     rospy.init_node('reactive_grasp_right')
     rospy.Subscriber('reactive_grasp_right', PoseStamped, self.update_frame)
     rospy.Subscriber('r_hand_pose', PoseStamped, self.update_curr_pose)
     rospy.Subscriber('wt_lin_move_right',Float32, self.linear_move)
     self.goal_out = rospy.Publisher('/reactive_grasp/right/goal', ReactiveGraspActionGoal)
     self.test_out = rospy.Publisher('/right_test_pose', PoseStamped)
     self.move_arm_out = rospy.Publisher('wt_move_right_arm_goals', PoseStamped)
     self.tf = TransformListener()
     self.tfb = TransformBroadcaster()
     self.wt_log_out = rospy.Publisher('wt_log_out', String )
Пример #49
0
 def __init__(self):
     self.br = TransformBroadcaster()
     self.pub_freq = 20.0
     self.parent_frame_id = "world"
     self.child1_frame_id = "reference1"
     self.child2_frame_id = "reference2"
     self.child3_frame_id = "reference3"
     self.child4_frame_id = "reference4"
     rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference2)
     rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference3)
     rospy.Timer(rospy.Duration(1 / self.pub_freq), self.reference4)
     rospy.sleep(1.0)
	def __init__(self):
		self.initialized = False		# make sure we don't perform updates before everything is setup
		rospy.init_node('pf')			# tell roscore that we are creating a new node named "pf"

		self.base_frame = "base_link"	# the frame of the robot base
		self.map_frame = "map"			# the name of the map coordinate frame
		self.odom_frame = "odom"		# the name of the odometry coordinate frame
		self.scan_topic = "scan"		# the topic where we will get laser scans from 

		self.n_particles = 300			# the number of particles to use

		self.d_thresh = 0.2				# the amount of linear movement before performing an update
		self.a_thresh = math.pi/6		# the amount of angular movement before performing an update

		self.laser_max_distance = 2.0	# maximum penalty to assess in the likelihood field model

		# TODO3: define additional constants if needed

		#this seems pretty self explanatory. 

		""" May need to adjust thresh values if robot is to be still.  
		May need to reduce number of particles.
		Dynamic Variables vs. Static Variables, will these be different?
		"""

		# Setup pubs and subs

		# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
		self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
		# publish the current particle cloud.  This enables viewing particles in rviz.
		self.particle_pub = rospy.Publisher("particlecloud", PoseArray)

		# laser_subscriber listens for data from the lidar
		self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

		# enable listening for and broadcasting coordinate transforms
		self.tf_listener = TransformListener()
		self.tf_broadcaster = TransformBroadcaster()

		self.particle_cloud = []

		self.current_odom_xy_theta = []

		# request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
		# TODO4: fill in the appropriate service call here.  The resultant map should be assigned be passed
		#		into the init method for OccupancyField
		""" Call the map """
		#rospy call to map "GetMap" imported from nav_msgs
		#set map as called map

		self.occupancy_field = OccupancyField(map)
		self.initialized = True
Пример #51
0
 def __init__(self):
     if World.tf_listener is None:
         World.tf_listener = TransformListener()
     self._lock = threading.Lock()
     self.surface = None
     self._tf_broadcaster = TransformBroadcaster()
     self._im_server = InteractiveMarkerServer('world_objects')
     self.clear_all_objects()
     self.relative_frame_threshold = 0.4
     # rospy.Subscriber("ar_pose_marker",
     #                  AlvarMarkers, self.receive_ar_markers)
     self.is_looking_for_markers = False
     self.marker_dims = Vector3(0.04, 0.04, 0.01)
     World.world = self
Пример #52
0
    def __init__(self):
        self.initialized = False        # make sure we don't perform updates before everything is setup
        rospy.init_node('pf')           # tell roscore that we are creating a new node named "pf"

        self.base_frame = "base_link"   # the frame of the robot base
        self.map_frame = "map"          # the name of the map coordinate frame
        self.odom_frame = "odom"        # the name of the odometry coordinate frame
        self.scan_topic = "scan"        # the topic where we will get laser scans from

        self.n_particles = 300          # the number of particles to use

        self.model_noise_rate = 0.15

        self.d_thresh = .2              # the amount of linear movement before performing an update
        self.a_thresh = math.pi / 6     # the amount of angular movement before performing an update

        self.laser_max_distance = 2.0   # maximum penalty to assess in the likelihood field model
        # TODO: define additional constants if needed

        # Setup pubs and subs

        # pose_listener responds to selection of a new approximate robot location (for instance using rviz)
        self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
        # publish the current particle cloud.  This enables viewing particles in rviz.
        self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
        # ?????
        # rospy.Subscriber('/simple_odom', Odometry, self.process_odom)
        # laser_subscriber listens for data from the lidar
        self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received, queue_size=10)

        # enable listening for and broadcasting coordinate transforms
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()
        self.particle_cloud = []
        self.current_odom_xy_theta = [] # [.0] * 3
        # self.initial_particles = self.initial_list_builder()
        # self.particle_cloud = self.initialize_particle_cloud()
        print(self.particle_cloud)
        # self.current_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)

        # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
        # TODO: fill in the appropriate service call here.  The resultant map should be assigned be passed
        #       into the init method for OccupancyField

        # for now we have commented out the occupancy field initialization until you can successfully fetch the map
        mapa = obter_mapa()
        self.occupancy_field = OccupancyField(mapa)
        # self.update_particles_with_odom(msg)
        self.initialized = True
Пример #53
0
    def __init__(self):
        self.initialized = False  # make sure we don't perform updates before everything is setup
        rospy.init_node('pf')  # tell roscore that we are creating a new node named "pf"

        self.base_frame = "base_link"  # the frame of the robot base
        self.map_frame = "map"  # the name of the map coordinate frame
        self.odom_frame = "odom"  # the name of the odometry coordinate frame
        self.scan_topic = "scan"  # the topic where we will get laser scans from

        self.n_particles = 30  # the number of particles to use

        self.d_thresh = 0.2  # the amount of linear movement before performing an update
        self.a_thresh = math.pi / 6  # the amount of angular movement before performing an update

        self.laser_max_distance = 2.0  # maximum penalty to assess in the likelihood field model

        # TODO: define additional constants if needed
        #set self.visualize_weights to True if you want to see a plot of xpos vs weights every time the particles are updated
        self.visualize_weights = True 

        # Setup pubs and subs

        # pose_listener responds to selection of a new approximate robot location (for instance using rviz)
        self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
        # publish the current particle cloud.  This enables viewing particles in rviz.
        self.rawcloud_pub = rospy.Publisher("rawcloud", PoseArray, queue_size=1)
        self.odomcloud_pub = rospy.Publisher("odomcloud", PoseArray, queue_size=1)
        self.lasercloud_pub = rospy.Publisher("lasercloud", PoseArray, queue_size=1)
        self.resamplecloud_pub = rospy.Publisher("resamplecloud", PoseArray, queue_size=1)
        self.finalcloud_pub = rospy.Publisher("finalcloud", PoseArray, queue_size=1)

        # laser_subscriber listens for data from the lidar
        self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

        # enable listening for and broadcasting coordinate transforms
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

        self.particle_cloud = []

        self.current_odom_xy_theta = []

        # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
        get_static_map = rospy.ServiceProxy('static_map', GetMap)
        self.occupancy_field = OccupancyField(get_static_map().map)
        self.robot_pose = Pose()
        self.initialized = True
Пример #54
0
	def __init__(self):
		print "ParticleFilter initializing "
		self.initialized = False		# make sure we don't perform updates before everything is setup
		rospy.init_node('comp_robo_project2')			# tell roscore that we are creating a new node named "pf"

		self.base_frame = "base_link"		# the frame of the robot base
		self.map_frame = "map"			# the name of the map coordinate frame
		self.odom_frame = "odom"		# the name of the odometry coordinate frame
		self.scan_topic = "scan"		# the topic where we will get laser scans from 

		self.n_particles = 200			# the number of paporticles to use

		self.d_thresh = 0.1				# the amount of linear movement before performing an update
		self.a_thresh = math.pi/12		# the amount of angular movement before performing an update

		self.laser_max_distance = 2.0	# maximum penalty to assess in the likelihood field model

		# Setup pubs and subs

		# pose_listener responds to selection of a new approximate robot location (for instance using rviz)
		self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
		# publish the current particle cloud.  This enables viewing particles in rviz.
		self.particle_pub = rospy.Publisher("particlecloud", PoseArray)
		self.pose_pub = rospy.Publisher("predictedPose", PoseArray)
		self.scan_shift_pub = rospy.Publisher("scanShift", PoseArray)

		# laser_subscriber listens for data from the lidar
		self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

		# enable listening for and broadcasting coordinate transforms
		self.tf_listener = TransformListener()
		self.tf_broadcaster = TransformBroadcaster()

		print "waiting for map server"
		rospy.wait_for_service('static_map')
		print "static_map service loaded"
		static_map = rospy.ServiceProxy('static_map', GetMap)
		worldMap = static_map()

		if worldMap:
			print "obtained map"

		# for now we have commented out the occupancy field initialization until you can successfully fetch the map
		self.occupancy_field = OccupancyField(worldMap.map)
		self.initialized = True
		print "ParticleFilter initialized"
Пример #55
0
    def __init__(self):

        if World.tf_listener == None:
            World.tf_listener = TransformListener()
        self._lock = threading.Lock()
        self.surface = None
        self._tf_broadcaster = TransformBroadcaster()
        self._im_server = InteractiveMarkerServer("world_objects")
        bb_service_name = "find_cluster_bounding_box"
        rospy.wait_for_service(bb_service_name)
        self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox)
        rospy.Subscriber("interactive_object_recognition_result", GraspableObjectList, self.receieve_object_info)
        self._object_action_client = actionlib.SimpleActionClient("object_detection_user_command", UserCommandAction)
        self._object_action_client.wait_for_server()
        rospy.loginfo("Interactive object detection action " + "server has responded.")
        self.clear_all_objects()
        # The following is to get the table information
        rospy.Subscriber("tabletop_segmentation_markers", Marker, self.receieve_table_marker)
    def __init__(self, use_dummy_transform=False):
        rospy.init_node('star_center_positioning_node')
        if use_dummy_transform:
            self.odom_frame_name = "odom_dummy"
        else:
            self.odom_frame_name = "odom"

        self.marker_locators = {}
        self.add_marker_locator(MarkerLocator(0,(0.0,0.0),0))
        self.add_marker_locator(MarkerLocator(1,(1.4/1.1,2.0/1.1),0))

        self.marker_sub = rospy.Subscriber("ar_pose_marker",
                                           ARMarkers,
                                           self.process_markers)
        self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10)
        self.star_pose_pub = rospy.Publisher("STAR_pose",PoseStamped,queue_size=10)
        self.continuous_pose = rospy.Publisher("STAR_pose_continuous",PoseStamped,queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()
Пример #57
0
    def __init__(self):
        self.initialized = False        # make sure we don't perform updates before everything is setup
        rospy.init_node('pf')           # tell roscore that we are creating a new node named "pf"

        self.base_frame = "base_link"   # the frame of the robot base
        self.map_frame = "map"          # the name of the map coordinate frame
        self.odom_frame = "odom"        # the name of the odometry coordinate frame
        self.scan_topic = "base_scan"        # the topic where we will get laser scans from

        self.n_particles = 500          # the number of particles to use

        self.d_thresh = 0.2             # the amount of linear movement before performing an update
        self.a_thresh = math.pi/6       # the amount of angular movement before performing an update

        self.laser_max_distance = 2.0   # maximum penalty to assess in the likelihood field model

        self.sigma = 0.08                # guess for how inaccurate lidar readings are in meters

        # Setup pubs and subs

        # pose_listener responds to selection of a new approximate robot location (for instance using rviz)
        self.pose_listener = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.update_initial_pose)
        # publish the current particle cloud.  This enables viewing particles in rviz.
        self.particle_pub = rospy.Publisher("particlecloud", PoseArray, queue_size=10)
        self.marker_pub = rospy.Publisher("markers", MarkerArray, queue_size=10)

        # laser_subscriber listens for data from the lidar
        self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received)

        # enable listening for and broadcasting coordinate transforms
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()

        self.particle_cloud = []

        self.current_odom_xy_theta = []

        # request the map from the map server, the map should be of type nav_msgs/OccupancyGrid
        self.map_server = rospy.ServiceProxy('static_map', GetMap)
        self.map = self.map_server().map
        # for now we have commented out the occupancy field initialization until you can successfully fetch the map
        self.occupancy_field = OccupancyField(self.map)
        self.initialized = True
    def __init__(self, use_dummy_transform=False):
        rospy.init_node("star_center_positioning_node")

        self.robot_name = rospy.get_param("~robot_name", "")
        self.odom_frame_name = self.robot_name + "_odom"
        # print self.odom_frame_name
        self.marker_locators = {}
        self.add_marker_locator(MarkerLocator(0, (-6 * 12 * 2.54 / 100.0, 0), 0))
        self.add_marker_locator(MarkerLocator(1, (0.0, 0.0), pi))
        self.add_marker_locator(MarkerLocator(2, (0.0, 10 * 12 * 2.54 / 100.0), 0))
        self.add_marker_locator(MarkerLocator(3, (-6 * 12 * 2.54 / 100.0, 6 * 12 * 2.54 / 100.0), 0))

        self.pose_correction = rospy.get_param("~pose_correction", 0.0)

        self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers)
        self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10)
        self.star_pose_pub = rospy.Publisher("STAR_pose", PoseStamped, queue_size=10)
        self.continuous_pose = rospy.Publisher("STAR_pose_continuous", PoseStamped, queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()
Пример #59
0
	def __init__(self):
		self.initialized = False
		rospy.init_node('my_amcl')
		print "MY AMCL initialized"
		# todo make this static
		self.n_particles = 100
		self.alpha1 = 0.2
		self.alpha2 = 0.2
		self.alpha3 = 0.2
		self.alpha4 = 0.2

		self.d_thresh = 0.2
		self.a_thresh = math.pi/6

		self.z_hit = 0.5
		self.z_rand = 0.5
		self.sigma_hit = 0.2

		self.laser_max_distance = 2.0

		self.laser_subscriber = rospy.Subscriber("scan", LaserScan, self.scan_received);
		self.tf_listener = TransformListener()
		self.tf_broadcaster = TransformBroadcaster()
		self.particle_pub = rospy.Publisher("particlecloud", PoseArray)
		self.particle_cloud = []
		self.last_transform_valid = False
		self.particle_cloud_initialized = False
		self.current_odom_xy_theta = []

		# request the map
		rospy.wait_for_service("static_map")
		static_map = rospy.ServiceProxy("static_map", GetMap)
		try:
			self.map = static_map().map
		except:
			print "error receiving map"

		self.create_occupancy_field()
		self.initialized = True