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()
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()
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 __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)
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,
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)
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, 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()
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()
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, 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()
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()
def __init__(self):
# once everything is setup initialized will be set to true
self.initialized = False
# initialize this particle filter node
rospy.init_node('turtlebot3_particle_filter')
# set the topic names and frame names
self.base_frame = "base_footprint"
self.map_topic = "map"
self.odom_frame = "odom"
self.scan_topic = "scan"
# inialize our map
self.map = OccupancyGrid()
# create LikelihoodField object
self.likelihood_field = LikelihoodField()
# the number of particles used in the particle filter
self.num_particles = 10000
# initialize the particle cloud array
self.particle_cloud = []
# initialize the estimated robot pose
self.robot_estimate = Pose()
# set threshold values for linear and angular movement before we preform an update
self.lin_mvmt_threshold = 0.2
self.ang_mvmt_threshold = (np.pi / 6)
self.odom_pose_last_motion_update = None
# Setup publishers and subscribers
# publish the current particle cloud
self.particles_pub = rospy.Publisher("particle_cloud", PoseArray, queue_size=10)
# publish the estimated robot pose
self.robot_estimate_pub = rospy.Publisher("estimated_robot_pose", PoseStamped, queue_size=10)
# subscribe to the map server
rospy.Subscriber(self.map_topic, OccupancyGrid, self.get_map)
# subscribe to the lidar scan from the robot
rospy.Subscriber(self.scan_topic, LaserScan, self.robot_scan_received)
# enable listening for and broadcasting corodinate transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
# sleep to get map data before initializing particle cloud
rospy.sleep(1)
# intialize the particle cloud
self.initialize_particle_cloud()
self.initialized = True
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)
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()
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()
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}')
def main():
rospy.init_node('my_broadcaster')
b = TransformBroadcaster()
translation = (0.0, 0.0, 0.0)
rotation = (0.0, 0.0, 0.0, 1.0)
rate = rospy.Rate(5) # 5hz
x, y = 0.0, 0.0
while not rospy.is_shutdown():
if x >= 2:
x, y = 0.0, 0.0
x += 0.1
y += 0.1
translation = (x, y, 0.0)
b.sendTransform(translation, rotation, Time.now(), '/odom', '/world')
b.sendTransform(translation, rotation, Time.now(), '/laser',
'/chassis')
b.sendTransform(translation, rotation, Time.now(), '/camera1',
'/chassis')
rate.sleep()
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 __init__(self): self.tl = TransformListener() self.br = TransformBroadcaster()
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()
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
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')
def move_robot_model_rotate():
translation = (0.0, 0.0, 0.0)
rotation = (0.0, 3.0, 0.0, 1.0)
b = TransformBroadcaster()
b.sendTransform(translation, rotation, Time.now(), 'base_link', '/map')
b.sendTransform(translation, rotation, Time.now(), 'camera_link', '/map')
b.sendTransform(translation, rotation, Time.now(),
'camera_link_normalized', '/map')
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)
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.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 = {}
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())
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)
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()
