def __init__(self):
self._controller_state = ControlMode.OFF
self._btn_remap = rospy.get_param("~mappings", {})
self._wrench_polynomial = rospy.get_param("~wrench_polynomial", {
"x": [1, 0],
"y": [1, 0],
"z": [1, 0],
"r": [1, 0]
})
self._switch_trigger = 0
self._joy_polynomial = {
"x": [1000, 0],
"y": [1000, 0],
"z": [500, 500],
"r": [1000, 0]
}
self._man = ManualControl() # manual control message
self._man.z = 500.0 # default 0 throttle value
self._man_pub = rospy.Publisher("mavros/manual_control/send",
ManualControl,
queue_size=10)
self._tf_buff = Buffer()
TransformListener(
self._tf_buff
) # listen for TFs to transform between wrench and base_link
self._serv = rospy.ServiceProxy("controller/set_control_mode",
SetControlMode)
rospy.Subscriber('controller/mode', ControlMode, self._update_mode)
rospy.Subscriber(
'joy', Joy,
self._joy_callback) # we listen on joystick for button commands
rospy.Subscriber('wrench/target', WrenchStamped, self._wrench_callback
) # listen on input_stamped for commanded wrench
def __init__(self, handeye_parameters):
self.handeye_parameters = handeye_parameters
# tf structures
self.tfBuffer = Buffer()
"""
used to get transforms to build each sample
:type: tf2_ros.Buffer
"""
self.tfListener = TransformListener(self.tfBuffer)
"""
used to get transforms to build each sample
:type: tf2_ros.TransformListener
"""
self.tfBroadcaster = TransformBroadcaster()
"""
used to publish the calibration after saving it
:type: tf.TransformBroadcaster
"""
# internal input data
self.samples = []
"""
def __init__(self, name):
super().__init__(name)
fill_map_param = self.declare_parameter('fill_map', True)
# Init map related elements
self.map = [-1] * MAP_WIDTH * MAP_HEIGHT
self.map_publisher = self.create_publisher(
OccupancyGrid,
'/map',
qos_profile=QoSProfile(
depth=1,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
))
self.tf_publisher = StaticTransformBroadcaster(self)
tf = TransformStamped()
tf.header.stamp = self.get_clock().now().to_msg()
tf.header.frame_id = 'map'
tf.child_frame_id = 'odom'
tf.transform.translation.x = 0.0
tf.transform.translation.y = 0.0
tf.transform.translation.z = 0.0
self.tf_publisher.sendTransform(tf)
# Init laser related elements
if fill_map_param.value:
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.scanner_subscriber = self.create_subscription(
LaserScan, '/scan', self.update_map, 1)
# Start publishing the map
self.publish_map()
self.create_timer(1, self.publish_map)
def __init__(self):
self._datum = rospy.get_param("~datum", None)
if self._datum is not None and len(self._datum) < 3:
self._datum += [0.0]
self._use_datum = self._datum is not None
self._earth_frame_id = rospy.get_param("~earth_frame_id", "earth")
self._utm_frame_id = rospy.get_param("~utm_frame_id", "utm")
self._map_frame_id = rospy.get_param("~map_frame_id", "map")
self._odom_frame_id = rospy.get_param("~odom_frame_id", "odom")
self._body_frame_id = rospy.get_param("~base_frame_id",
"base_link") # currently unused
self._tf_broadcast_rate = rospy.get_param("~broadcast_rate", 10.0)
self._serv = rospy.Service("~set_datum", SetDatum, self._set_datum)
self._tf_buff = Buffer()
TransformListener(self._tf_buff)
self._tf_bcast = TransformBroadcaster()
self._last_datum = None
self._static_map_odom = rospy.get_param("~static_map_odom", False)
self._odom_updated = False
self._update_odom_serv = rospy.Service("~set_odom", Trigger,
self._handle_set_odom)
if not self._use_datum:
rospy.Subscriber("gps_datum", NavSatFix, self._handle_datum)
if not self._static_map_odom:
rospy.Subscriber("waterlinked/pose_with_cov_stamped",
PoseWithCovarianceStamped, self._handle_odom_tf)
else:
StaticTransformBroadcaster().sendTransform(
TransformStamped(
Header(0, rospy.Time.now(), self._map_frame_id),
self._odom_frame_id, None, Quaternion(0, 0, 0, 1)))
self._map_odom_tf = None
rospy.Timer(rospy.Duration.from_sec(1.0 / self._tf_broadcast_rate),
self._send_tf)
def __init__(self):
self._estimated_state_pub = rospy.Publisher("imc/estimated_state",
EstimatedState,
queue_size=5)
self._estimated_state_msg = EstimatedState()
self._geo_converter = rospy.ServiceProxy("convert_points",
ConvertGeoPoints)
self._plan_db = dict()
self._current_plan = PlanDB()
self._tf_buffer = Buffer()
TransformListener(self._tf_buffer)
rospy.Subscriber("gps", NavSatFix, self._handle_gps)
rospy.Subscriber("odometry", Odometry, self._handle_pose)
rospy.Subscriber("imc/goto_waypoint", Pose, self._handle_goto)
rospy.Subscriber("imc/plan_control", PlanControl,
self._handle_plan_control)
rospy.Subscriber("imc/abort", Empty, self._handle_abort)
rospy.Subscriber("imc/imc_heartbeat", Empty,
self._handle_imc_heartbeat)
rospy.Subscriber("imc/plan_db", PlanDB, self._handle_plan_db)
rospy.Timer(rospy.Duration(1, 0), self._send_estimated_state)
self._waypoint_serv = rospy.Service("load_waypoints", InitWaypointSet,
self._send_goal)
self._action_client = actionlib.SimpleActionClient(
"follow_waypoints", FollowWaypointsAction)
self._mode_client = rospy.ServiceProxy("controller/set_control_mode",
SetControlMode)
self._plan_db_pub = rospy.Publisher("imc/plan_db",
PlanDB,
queue_size=10)
self._marker_pub = rospy.Publisher("current_waypoints",
WaypointSet,
queue_size=1)
def __init__(self):
self.node_name = "Echo"
self.thread_lock = threading.Lock()
self.lastHeader = None
self.buff = Buffer()
self.listener = TransformListener(self.buff)
self.r = rospy.Rate(10)
thread = threading.Thread(target=self.latestTransform)
thread.start()
self.sub = rospy.Subscriber("/kinect2/sd/points",\
PointCloud2, self.cb, queue_size=1)
self.srv = rospy.ServiceProxy('/segment_scene', SegmentScene)
self.detsrv = rospy.ServiceProxy('/detect_models_soda',\
DetectModels)
self.tablepub = rospy.Publisher("mytable", \
PolygonStamped, queue_size=1)
self.pub = rospy.Publisher("~echo_image",\
PointCloud2, queue_size=1)
#self.tablepub = rospy.Publisher("table",\
# PointCloud2, queue_size=1)
#self.nottablepub = rospy.Publisher("tablenot",\
# PointCloud2, queue_size=1)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def __init__(self, gt_frame, est_frame, max_time_between=0.01):
self.gt_frame = gt_frame
self.est_frame = est_frame
self.frames = [gt_frame, est_frame]
self.tf_buffer = Buffer(cache_time=rospy.Duration(max_time_between))
self.__tf_listener = TransformListener(self.tf_buffer)
def get_buffer():
global _buffer, _listener
with _lock:
if _buffer is None:
server = rospy.get_param('~tf_server', None)
if server:
check_frequency = rospy.get_param('~tf_check_frequency', None)
timeout_padding = rospy.get_param('~tf_timeout_padding', 2.0)
_buffer = BufferClient(
server, check_frequency,
rospy.Duration.from_sec(timeout_padding))
rospy.loginfo(
'Using tf buffer client (server %s, timeout padding %.3g s).',
server, timeout_padding)
else:
cache_time = rospy.get_param('~tf_cache_time', 10.0)
queue_size = rospy.get_param('~tf_queue_size', None)
buff_size = rospy.get_param('~tf_buff_size', 65536)
_buffer = Buffer(rospy.Duration(cache_time))
_listener = TransformListener(_buffer, queue_size, buff_size)
rospy.loginfo(
'Using local tf buffer (cache %.3g s, queue size %s, buffer size %s).',
cache_time, queue_size, buff_size)
return _buffer
def __init__(self):
# Read Parameters From YAML File
self.turtlebot_odom_topic = get_param('turtlebot_odom_topic')
self.turtlebot_base_footprint_frame = get_param(
'turtlebot_base_footprint_frame')
self.map_frame = get_param('map_frame')
self.drone_odom_frame = get_param('drone_odom_frame')
self.join_tree_srv_name = get_param('join_tree_srv')
self.debug = get_param('debug')
# TF Broadcaster
self.tf_broadcast = TransformBroadcaster()
# TF Listener
self.tf_listen = Buffer()
self.tf_listener = TransformListener(self.tf_listen)
# Subscribers
Subscriber(self.turtlebot_odom_topic, Odometry, self.turtlebot_odom_cb)
# Services for Joining the Drone TF Tree to Main Tree (Localization)
self.join_tree_service = Service(self.join_tree_srv_name, JointTrees,
self.join_tree_srv_function)
# Parameters
self.drone_tf = TransformStamped()
self.turtlebot_tf = TransformStamped()
self.turtlebot_tf.header.frame_id = self.map_frame
self.turtlebot_tf.child_frame_id = self.turtlebot_base_footprint_frame
self.drone_tf.header.frame_id = self.map_frame
self.drone_tf.child_frame_id = self.drone_odom_frame
self.drone_tf.transform.rotation.w = 1.0
self.turtlebot_tf.transform.rotation.w = 1.0
# Flags
self.join_trees = False # Set True When The Two TF Trees are Joint in One
def __init__(self):
# TF Listener
self.tf = Buffer()
self.tf_listener = TransformListener(self.tf)
# Flags
self.initialize = False
# Parameters
self.last_t = 0.0
self.lin_i_x = 0.0
self.lin_i_y = 0.0
self.kp = 0.0
self.ki = 0.0
self.kd = 0.0
self.error = 0.15
self.max_speed = 1.8
self.velocity = Twist()
self.empty = Twist()
# FIXME: Parameter from YAML File
self.drone_base_link = get_param('drone_base_link')
self.map_frame = get_param('map_frame')
self.drone_speed_topic = get_param('drone_speed_topic')
# Publish Speed Topic
self.cmd_topic = Publisher(self.drone_speed_topic,
Twist,
queue_size=10)
def __init__(self):
super().__init__('product_distance_server')
self.srv = self.create_service(ProductDistance, 'product_distance',
self.product_distance_callback)
self.tfBuffer = Buffer()
self.listener = TransformListener(self.tfBuffer, self)
self.get_logger().info("Product Distance Server is ready")
def align_cb(userdata, default_goal):
rospy.logdebug("Entered align base callback.")
align_goal = MoveBaseGoal()
tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer)
tf_stamped = tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0), rospy.Duration(10))
# we don't want the robot to translate, only to rotate
align_goal.target_pose.pose.position.x = tf_stamped.transform.translation.x
align_goal.target_pose.pose.position.y = tf_stamped.transform.translation.y
# get orientation to face waypoint 2
desired_yaw = atan2(userdata.al_cb_waypoints[1].pose.position.y - align_goal.target_pose.pose.position.y,
userdata.al_cb_waypoints[1].pose.position.x - align_goal.target_pose.pose.position.x)
q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, desired_yaw)
align_goal.target_pose.pose.orientation.x = q[0]
align_goal.target_pose.pose.orientation.y = q[1]
align_goal.target_pose.pose.orientation.z = q[2]
align_goal.target_pose.pose.orientation.w = q[3]
align_goal.target_pose.header.frame_id = 'map'
align_goal.target_pose.header.stamp = rospy.Time.now()
return align_goal
def __init__(self, buffer_size=2):
self.tfBuffer = Buffer(rospy.Duration(buffer_size))
self.tf_listener = TransformListener(self.tfBuffer)
self.tf_static = StaticTransformBroadcaster()
self.tf_frequency = rospy.Duration(1.0)
self.broadcasting_frames = []
self.broadcasting_frames_lock = Lock()
rospy.sleep(0.1)
def __init__(self, name):
super().__init__(name)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.create_timer(SAMPLE_PERIOD, self.log_pose)
self.time_since_start = 0
self.file = open('odom_log.csv', 'w')
def __init__(self):
smach.State.__init__(self, outcomes=['wait_over'],
input_keys=['wait_target_wp', 'wait_waypoints'],
output_keys=['wait_target_wp'])
self.move_base_srv = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
def __init__(self):
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
self.tf_broadcaster = TransformBroadcaster()
self.camera_info_topic = rospy.get_param("~camera_info_topic", "")
self.global_frame_id = rospy.get_param("~global_frame_id", "odom")
self.resource_directory = rospy.get_param("~resource_directory", "")
if self.resource_directory == "":
raise ValueError(
"Need to specify the '~resource_directory' parameter")
self.ar_tags_config = rospy.get_param("~ar_tags_config", "")
if self.ar_tags_config == "":
raise ValueError("Need to specify the '~ar_tags_config' parameter")
self.load_config(self.ar_tags_config)
self.tracks = {}
self.camera_info = None
for entity_id, entity in self.config.items():
track = uwds3_msgs.msg.SceneNode()
track.label = self.config[entity_id]["label"]
track.description = self.config[entity_id]["description"]
track.is_located = True
track.has_shape = True
shape = uwds3_msgs.msg.PrimitiveShape()
shape.type = uwds3_msgs.msg.PrimitiveShape.MESH
position = self.config[entity_id]["position"]
orientation = self.config[entity_id]["orientation"]
q = quaternion_from_euler(orientation["x"], orientation["y"],
orientation["z"], "rxyz")
shape.pose.position.x = position["x"]
shape.pose.position.y = position["y"]
shape.pose.position.z = position["z"]
shape.pose.orientation.x = q[0]
shape.pose.orientation.y = q[1]
shape.pose.orientation.z = q[2]
shape.pose.orientation.w = q[3]
shape.mesh_resource = "file://" + self.resource_directory + self.config[
entity_id]["mesh_resource"]
track.shapes = [shape]
self.tracks[entity_id] = track
self.camera_info_subscriber = rospy.Subscriber(
self.camera_info_topic, sensor_msgs.msg.CameraInfo,
self.camera_info_callback)
self.ar_track_subscriber = rospy.Subscriber(
"ar_pose_marker", ar_track_alvar_msgs.msg.AlvarMarkers,
self.observation_callback)
self.tracks_publisher = rospy.Publisher(
"tracks", uwds3_msgs.msg.SceneChangesStamped, queue_size=1)
def init(tf_buffer_size=15):
"""
If you want to specify the buffer size, call this function manually, otherwise don't worry about it.
:param tf_buffer_size: in secs
:type tf_buffer_size: int
"""
global tfBuffer, tf_listener
tfBuffer = Buffer(rospy.Duration(tf_buffer_size))
tf_listener = TransformListener(tfBuffer)
rospy.sleep(5.0)
def rosInterface(self):
# global sensor hearbeat topics
self.lidarTopic = str(rospy.get_param('lidar_beat', '/scan'))
self.seekTopic = str(
rospy.get_param('seek_beat', '/seek_camera/filteredImage'))
self.realSenseRGBTopic = str(
rospy.get_param('realsense_rgb_beat', '/camera/color/image_raw'))
self.realSenseDepthTopic = str(
rospy.get_param('realsense_depth_beat',
'/camera/depth/image_rect_raw'))
self.transformedIMUTopic = str(
rospy.get_param('transform_imu_beat', '/imu'))
# global real time status update topics
self.humanLocalizeTopic = str(
rospy.get_param('human_beat', '/RelLocHeartbeat'))
self.humanDetectionTopic = str(
rospy.get_param('detection_beat', '/darknet_ros/detection_image'))
self.humanFilterTopic = str(
rospy.get_param('filter_beat', '/world_state'))
# the heartbeats to listen to subscribers
self.lidarSub_ = rospy.Subscriber(self.lidarTopic, LaserScan,
self.lidarBeatCallback)
self.seekSub_ = rospy.Subscriber(self.seekTopic, Image,
self.seekCallback)
self.realSenseRGBSub_ = rospy.Subscriber(self.realSenseRGBTopic, Image,
self.realSenseRGBCallback)
self.realSenseDepthSub_ = rospy.Subscriber(self.realSenseDepthTopic,
Image,
self.realSenseDepthCallback)
self.transformedIMUSub_ = rospy.Subscriber(self.transformedIMUTopic,
Imu, self.imuCallback)
self.humanLocalizeSub_ = rospy.Subscriber(self.humanLocalizeTopic,
Empty,
self.humanLocalizeCallback)
self.humanDetectionSub_ = rospy.Subscriber(self.humanDetectionTopic,
Image,
self.humanDetectionCallback)
self.humanFilterSub_ = rospy.Subscriber(self.humanFilterTopic,
WorldState,
self.humanFilterCallback)
# status publisher
self.statusTopic_ = str(
rospy.get_param("health_status", "/health_status"))
self.outputMsg = watchStatus()
self.healthPub_ = rospy.Publisher(self.statusTopic_,
watchStatus,
queue_size=10)
# tf listener
self.tfBuffer = Buffer()
self.slamListener = TransformListener(self.tfBuffer)
def __init__(self):
smach.State.__init__(
self,
outcomes=['target_set', 'no_new_object'],
input_keys=[
'sot_object_array', 'sot_ork_frame', 'sot_picked_objects',
'sot_target_object', 'sot_grasp_target_pose'
],
output_keys=['sot_target_object', 'sot_grasp_target_pose'])
self.ork_srv = rospy.ServiceProxy('get_object_info',
GetObjectInformation)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
def __init__(self, name):
self._retires = 3
self._dist_fac = 0.1
self._preempt = False
self._action_server = actionlib.SimpleActionServer(name, MoveBaseAction, execute_cb = self.execute_cb, auto_start = False)
self._action_server.register_preempt_callback(self.preempt_cb)
self._action_client = actionlib.SimpleActionClient('/move_base/move', MoveBaseAction)
self._tf_buffer = Buffer()
self._tf_listener = TransformListener(self._tf_buffer)
self._action_server.start()
self._action_client.wait_for_server()
def __init__(self):
smach.State.__init__(self, outcomes=['monitoring_done', 'wp2_case', 'monitor_failed'],
input_keys=['ma_current_attempt', 'ma_attempt_limit', 'ma_target_wp', 'ma_waypoints',
'ma_wp_str'],
output_keys=['ma_current_attempt', 'ma_target_wp'])
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True)
self.face_cmd = rospy.Publisher('/face_mode', UInt8, queue_size=1, latch=True)
self.grasp_distance = 1.6
def __init__(self):
_node = Node("tf2twist")
qos_profile = QoSProfile(depth=10)
# loop_rate = _node.create_rate(100)
tfBuffer = Buffer()
listener = TransformListener(tfBuffer, _node, qos=qos_profile)
twist_data = geometry_msgs.msg.Twist()
pub_cmd_vel = _node.create_publisher(geometry_msgs.msg.Twist,
"cmd_vel", qos_profile)
# self.transfromstamped =
try:
while rclpy.ok():
rclpy.spin_once(_node)
now = _node.get_clock().now() - rclpy.duration.Duration(
seconds=0, nanoseconds=1000000)
try:
trans = tfBuffer.lookup_transform(
'odom_frame', 'base_link', now,
rclpy.duration.Duration(seconds=0, nanoseconds=0))
# print(trans)
# print(trans.transform.rotation.x)
roll, pitch, yaw = self.quaternion_to_euler(
trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
# print(yaw)
if (yaw > 0.3):
twist_data.angular.z = 70.0 #yaw*125*1.3
elif (yaw < -0.3):
twist_data.angular.z = -70.0 #yaw*125*1.3
else:
twist_data.angular.z = 0.0
pub_cmd_vel.publish(twist_data)
except (LookupException, LookupError, ConnectionAbortedError,
ConnectionError, ConnectionRefusedError,
ConnectionResetError, ExtrapolationException,
ConnectivityException) as e:
# print(e)
pass
except (KeyboardInterrupt):
pass
def __init__(self):
# TF Listener
self.tf = Buffer()
self.tf_listener = TransformListener(self.tf)
# Flags
self.initialize = False
self.marker_detected = False # True if Drone Camera has detected a Tag
self.recovery_behavior = False # Used in recovery() Function if Marker is Lost
# PID Parameters
self.last_t = 0.0
self.lin_i_x = 0.0
self.lin_i_y = 0.0
self.lin_i_z = 0.0
self.rot_i_z = 0.0
# Gains ( with Initial Values)
self.kp = 0.3
self.ki = 0.01
self.kd = 0.06
self.r_kp = 3.0
self.r_ki = 1.5
self.r_kd = 0.57
self.r_kp_s = 0.6
self.r_ki_s = 0.13
self.r_kd_s = 0.07
self.error = 0.25
self.max_speed = 1.0
self.max_z_speed = 1.5
# Parameter from YAML File
self.drone_base_link = get_param('drone_base_link')
self.drone_base_footprint = get_param('drone_base_footprint')
self.map_frame = get_param('map_frame')
self.ar_pose_frame = get_param('ar_pose_frame')
self.drone_camera_frame = get_param('drone_camera_frame')
self.drone_speed_topic = get_param('drone_speed_topic')
self.ar_pose_topic = get_param('ar_pose_topic')
self.time_to_considered_marker_lost = get_param(
'time_to_considered_marker_lost')
# Parameters
self.last_time_marker_seen = 0.0 # Hold the time that Tag last seen, for Setting Marker Detected flag ON/OFF
# Ar Marker Topic Subscriber
self.marker_subscriber = Subscriber('/ar_pose_marker', ARMarker,
self.marker_cb)
# Timer for Checking if Marker is Lost
self.check_marker_timer = Timer(Duration(0.05),
self.check_marker_existence)
def __init__(self):
super(TFRepublisher, self).__init__('tf2_web_republisher')
self.tf_transform_server = ActionServer(
node=self,
action_type=TFSubscription,
action_name='tf2_web_republisher',
execute_callback=self.goal_cb,
cancel_callback=self.cancel_cb)
self.republish_service = self.create_service(RepublishTFs,
'republish_tfs',
self.request_cb)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, node=self)
self.active_clients = {}
def __init__(self):
# Parameters from YAML File
self.drone_base_footprint_frame = rospy.get_param('drone_base_footprint')
self.drone_base_link_frame = rospy.get_param('drone_base_link')
self.map_frame = rospy.get_param('map_frame')
self.control_drone_srv_name = rospy.get_param('drone_control_srv')
# TF Listener for Getting Drone Position
self.tf = Buffer()
self.tf_listener = TransformListener(self.tf)
# String with Num. of Command that will be sent to Drone Service
self.drone_command = ''
# Control Drone Service
self.control_drone = rospy.ServiceProxy(self.control_drone_srv_name, ControlDrone)
def __init__(self):
smach.State.__init__(self,
outcomes=['go_back'],
input_keys=['sg_waypoints'],
output_keys=['sg_target_wp'])
self.tts_pub = rospy.Publisher('sara_tts',
String,
queue_size=1,
latch=True)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
self.face_cmd = rospy.Publisher('/face_mode',
UInt8,
queue_size=1,
latch=True)
def __init__(self, robot_urdf):
self.global_frame_id = 'map'
self.bullet_node_id_map = {}
# Initialize tf2 buffer
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
self.robot_urdf = robot_urdf
self.base_frame_id = "base_footprint"
sleep(1.)
_, t, q = self.get_transform_from_tf2(self.global_frame_id,
self.base_frame_id)
self.bullet_node_id_map[self.base_frame_id] = p.loadURDF(
self.robot_urdf, t, q)
self.init_robot_joint_map()
self.joint_state_subscriber = rospy.Subscriber(
"/joint_states", JointState, self.update_robot_joints)
def __init__(self):
smach.State.__init__(self,
outcomes=['drop_pose_acquired'],
output_keys=['drop_pose'])
self.tabletop_sub = rospy.Subscriber('/object_recognition/table_array',
TableArray,
self.tabletop_cb,
queue_size=10)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
self.msg_received = False
self.mutex = threading.Lock()
self.tables = TableArray()
def __init__(self):
self.tfBuffer = Buffer(rospy.Duration(10))
self.tf_listener = TransformListener(self.tfBuffer)
self.barcode_pose_sub = rospy.Subscriber('barcode/pose',
Barcode,
self.barcode_sub,
queue_size=100)
self.done = rospy.Service('barcode_evaluation/done', Trigger,
self.done_cb)
self.marker_pub = rospy.Publisher('wrong_codes', Marker, queue_size=10)
self.barcodes = {}
self.tp = {}
self.fp = {}
self.data = Data()
self.load_ground_truth()
rospy.loginfo('rdy to evaluate barcode detection')
rospy.sleep(1)
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.tf2_broadcaster = TransformBroadcaster()
self.tf2_buffer = Buffer()
self.tf2_listener = TransformListener(self.tf2_buffer)
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)
self.camera_frame = cam_info.header.frame_id
if self.camera_frame.startswith("/"):
self.camera_frame = self.camera_frame[1:]
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)
self.headpose_publisher = rospy.Publisher("/subjects/headpose", MSG_HeadposeList, queue_size=3)
self.landmark_publisher = rospy.Publisher("/subjects/landmarks", MSG_LandmarksList, 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)
