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'
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'