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,
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, 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 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 __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()
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 __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):
rospy.init_node('left_utilitiy_frame_source')
self.updater = rospy.Service('l_utility_frame_update', FrameUpdate,
self.update_frame)
self.tfb = TransformBroadcaster()
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):
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 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):
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()
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
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
def __init__(self): self.tl = TransformListener() self.br = 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, 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()
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
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
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))
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):
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):
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 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 __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):
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 = {}
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()
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()
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)
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()
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()
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()
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
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)
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
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 )
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 __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 )
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
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
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
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
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"
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()
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()
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
