def __init__(self, transforms):
self.t_front = transforms[0].header.stamp
self.t_back = transforms[-1].header.stamp
self.buff = Buffer(cache_time=(self.t_back - self.t_front),
debug=False)
for transform in transforms:
self.buff.set_transform(transform, '/auth')
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
class OfflineTfBuffer():
def __init__(self, transforms):
self.t_front = transforms[0].header.stamp
self.t_back = transforms[-1].header.stamp
self.buff = Buffer(cache_time=(self.t_back - self.t_front),
debug=False)
for transform in transforms:
self.buff.set_transform(transform, '/auth')
def LookupTransform(self, target_frame, source_frame, times):
return [
self.buff.lookup_transform(target_frame, source_frame, t)
for t in times
]
def AvailableTimeRange(self, target_frame, source_frame, times):
if times[0] > self.t_back or times[-1] < self.t_front:
return []
idx_front = 0
idx_back = len(times) - 1
while times[idx_front] < self.t_front:
idx_front += 1
while times[idx_back] > self.t_back:
idx_back -= 1
return (idx_front, idx_back + 1)
def CanTransform(self, target_frame, source_frame, times):
return self.buff.can_transform(
target_frame, source_frame, times[0]) and self.buff.can_transform(
target_frame, source_frame, times[-1])
class TransformHandler():
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)
#self.warn_timer = rospy.Timer(rospy.Duration(5), self.__warn_timer_cb)
def __warn_timer_cb(self, evt):
available_frames = self.tf_buffer.all_frames_as_string()
avail = True
for frame in self.frames:
if frame not in available_frames:
rospy.logwarn('Frame {} has not been seen yet'.format(frame))
avail = False
if avail:
self.warn_timer.shutdown()
def get_transform(self, fixed_frame, target_frame):
# caller should handle the exceptions
return self.tf_buffer.lookup_transform(target_frame, fixed_frame,
rospy.Time(0))
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 __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 __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._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.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):
# 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, 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, 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)
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, 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):
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)
class DroneCommunication:
# Initialization Function
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)
# Get the Yaw Between Map and Drone Function
def get_drone_yaw(self):
# TODO: Read TF and Compute Yaw
try:
tf_ = self.tf.lookup_transform(self.drone_base_link_frame, self.map_frame,
rospy.Time(0), rospy.Duration(1.0))
(_, _, yaw) = euler_from_quaternion([tf_.transform.rotation.x, tf_.transform.rotation.y,
tf_.transform.rotation.z, tf_.transform.rotation.w])
return 180-np.rad2deg(yaw)
except tf.LookupException or tf.ConnectivityException or tf.ExtrapolationException:
return -1
# Get the Position in Map of Drone Function
def get_drone_pose(self):
# TODO: Read TF and Compute Yaw
try:
tf_ = self.tf.lookup_transform(self.map_frame, self.drone_base_link_frame,
rospy.Time(0), rospy.Duration(1.0))
(r1, r2, r3, _) = quaternion_matrix([tf_.transform.rotation.x, tf_.transform.rotation.y,
tf_.transform.rotation.z, tf_.transform.rotation.w])
pose = np.asmatrix([r1[0:3], r2[0:3], r3[0:3]]) * \
np.asmatrix([[tf_.transform.translation.x],[tf_.transform.translation.y],[tf_.transform.translation.z]])
(x, y, z) = (pose.item(0), pose.item(1), pose.item(2))
except tf.LookupException or tf.ConnectivityException or tf.ExtrapolationException:
return -1
return x, y, z
# Send SIGNAL to Control Drone Service to Execute Command
def sent_control_command(self, command):
rospy.wait_for_service(self.control_drone_srv_name)
try:
drone_execute_result = self.control_drone(command)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return drone_execute_result.status
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 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, 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):
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):
_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):
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):
# 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):
# 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)
class SimpleMapper(Node):
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 log_pose(self):
rotation = None
translation = None
try:
tf = self.tf_buffer.lookup_transform('odom', 'base_link',
Time(sec=0, nanosec=0))
rotation = quaternion_to_euler(tf.transform.rotation)[0]
translation = [
tf.transform.translation.x, tf.transform.translation.y
]
except (LookupException, ConnectivityException,
ExtrapolationException) as e:
print('No required transformation found: `{}`'.format(str(e)))
return
if translation[0] or translation[
1] or rotation or self.time_since_start:
self.time_since_start += SAMPLE_PERIOD
output = f'{self.time_since_start:.1f},{translation[0]:.3f},{translation[1]:.3f},{rotation:.2f}\n'
print(output)
self.file.write(output)
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
class SetObjectTarget(smach.State):
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 execute(self, ud):
rospy.logdebug("Entered 'SET_OBJECT_TARGET' state.")
new_target = ''
is_new_obj = False
# loop through all found objects
for i in range(len(ud.sot_object_array)):
# get object info, mainly its name
try:
req = GetObjectInformationRequest()
req.type = ud.sot_object_array[i].type
res = self.ork_srv(req)
# if the object hasn't been grasped yet, make the object the current grasp target
is_new_obj = True
if any(res.information.name in s for s in ud.sot_picked_objects):
is_new_obj = False
if is_new_obj:
new_target = res.information.name
except rospy.ServiceException:
rospy.logerr("GetObjectInformation service request failed.")
# if a new object has been detected
if new_target and is_new_obj:
# get the transform from ork's frame to odom
transform = self.tf_buffer.lookup_transform('odom', ud.sot_ork_frame, rospy.Time(0))
# get the object's pose in odom frame
ud.sot_grasp_target_pose = do_transform_pose(ud.sot_object_array[i].pose.pose, transform)
ud.sot_target_object = new_target
ud.sot_grasp_target_pose.pose.position.z += 0.08
ud.sot_grasp_target_pose.pose.position.x -= 0.15
print "OBJECT NAME : " + new_target
print "POSE : "
print ud.sot_grasp_target_pose
check_call([PATH_TO_PDF_CREATOR], shell=True)
check_call(['~/sara_ws/src/PDF_creator/create_pdf.sh'], shell=True)
return 'target_set'
# rospy.logerr("Could not get transform from " + ud.sot_ork_frame + " to 'odom'.")
return 'no_new_object'
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)
class GetDropPose(smach.State):
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 tabletop_cb(self, table_array):
self.mutex.acquire()
self.msg_received = True
self.tables = table_array
self.mutex.release()
def execute(self, ud):
loop_again = True
while loop_again:
self.mutex.acquire()
if self.msg_received:
loop_again = False
self.mutex.release()
rospy.sleep(rospy.Duration(1))
transform = self.tf_buffer.lookup_transform('odom', self.tables.header.frame_id, rospy.Time(0))
drop_pose_z = 1.5
tmp = PoseStamped()
lst = []
for t in range(len(self.tables.tables)):
tmp.pose = self.tables.tables[t].pose
tmp.header.frame_id = self.tables.header.frame_id
tmp.header.stamp = rospy.Time.now()
eval_pose = do_transform_pose(tmp, transform)
rpy = tf_conversions.transformations.euler_from_quaternion(eval_pose.pose.orientation)
if rpy[2]**2 > 0.90:
lst.append([(eval_pose.pose.position.z - drop_pose_z)**2, eval_pose])
lst.sort()
ud.drop_pose = lst[0][1]
print ud.drop_pose
return 'drop_pose_acquired'
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):
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()
class StartGuiding(smach.State):
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 execute(self, ud):
tts_msg = String()
rospy.sleep(rospy.Duration(5))
self.face_cmd.publish(YELLOW_FACE)
tts_msg.data = "I can not guide you back safely. I will go back to the stating location on my own."
self.tts_pub.publish(tts_msg)
tf_stamped = self.tf_buffer.lookup_transform('map', 'base_link', rospy.Time(0))
lst = []
for i in range(len(ud.sg_waypoints)):
distance = sqrt((tf_stamped.transform.translation.x - ud.sg_waypoints[i].pose.position.x)**2 +
(tf_stamped.transform.translation.y - ud.sg_waypoints[i].pose.position.y)**2)
lst.append([distance, i])
lst.sort()
if lst[0][1] == 0:
ud.sg_target_wp = 0
else:
ud.sg_target_wp = lst[0][1] - 1
return 'go_back'
