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 publish_odometry(): # AJC: make this configurable rate = rospy.Rate(30) odom_publisher = rospy.Publisher('adabot/odometry', Odometry, queue_size=10) odom_broadcaster = TransformBroadcaster() while not rospy.is_shutdown(): current_time = rospy.get_rostime() # Publish the transform over tf odom_transform = TransformStamped() odom_transform.header.stamp = current_time odom_transform.header.frame_id = 'odom' odom_transform.child_frame_id = 'base_link' odom_transform.transform.rotation.w = 1 odom_broadcaster.sendTransform(odom_transform) # Publish the odometry message over ROS odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = 'odom' odom.child_frame_id = 'base_link' # odom.pose.pose = 0 odom.twist.twist.linear.x = vx odom_publisher.publish(odom) rate.sleep()
class AUVTransformPublisher(Node): def __init__(self): super().__init__('auv_transform_publisher') qos_profile = QoSProfile(depth=10) self.state_subscriber = self.create_subscription( PoseStamped, '/triton/state', self.state_callback, 10) self.path_publisher = self.create_publisher(Path, '/triton/path', qos_profile) self.pose_array = [] self.broadcaster = TransformBroadcaster(self, qos=qos_profile) self.get_logger().info("AUVTransformPublisher successfully started!") def state_callback(self, msg): self.pose_array.append(msg) now = self.get_clock().now() p = msg.pose.position q = msg.pose.orientation odom_trans = TransformStamped() odom_trans.header.frame_id = msg.header.frame_id odom_trans.child_frame_id = 'base_link' odom_trans.header.stamp = now.to_msg() odom_trans.transform.translation.x = p.x odom_trans.transform.translation.y = p.y odom_trans.transform.translation.z = p.z odom_trans.transform.rotation = q self.broadcaster.sendTransform(odom_trans) path_msg = Path() path_msg.header.frame_id = msg.header.frame_id path_msg.header.stamp = now.to_msg() path_msg.poses = self.pose_array self.path_publisher.publish(path_msg)
def __init__(self): self.tf_broadcaster = TransformBroadcaster() self.ts = TransformStamped() self.ts.header.frame_id = 'world' self.ts.child_frame_id = 'drone' pose_sub = Subscriber('/dji_sdk/pose', PoseStamped, self.pose_callback)
def publish_odom(self): self.odom_broadcaster = TransformBroadcaster(self) # self.current_time = datetime.now().microsecond # compute odometry in a typical way given the velocities of the robot dt = self.time_step delta_x = (self.vx * math.cos(self.th) - self.vy * math.sin(self.th)) * dt delta_y = (self.vx * math.sin(self.th) + self.vy * math.cos(self.th)) * dt delta_th = self.vth * dt self.x += delta_x self.y += delta_y self.th += delta_th # since all odometry is 6DOF we'll need a quaternion created from yaw # odom_quat=[0.0,0.0,0.0,1.0] odom_quat = self.euler_to_quaternion(self.th, 0, 0) # first, we'll publish the transform over tf odom_transform = TransformStamped() odom_transform.header.stamp = self.get_clock().now().to_msg() # odom_transform.header.stamp = Time(seconds=self.getTime().now()).to_msg() odom_transform.header.frame_id = 'odom' odom_transform.child_frame_id = 'base_link' odom_transform.transform.rotation.x = odom_quat[0] odom_transform.transform.rotation.y = odom_quat[1] odom_transform.transform.rotation.z = odom_quat[2] odom_transform.transform.rotation.w = odom_quat[3] odom_transform.transform.translation.x = self.x odom_transform.transform.translation.y = self.y odom_transform.transform.translation.z = 0.0 # self.get_logger().info('base_link to odom being published : %d' % self.get_clock().now()) self.odom_broadcaster.sendTransform(odom_transform) odom = Odometry() odom.header.stamp = self.get_clock().now().to_msg() # odom.header.stamp = Time(seconds=self.robot.getTime()).to_msg() odom.header.frame_id = "odom" odom.child_frame_id = "base_link" # set the position odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.orientation.x = odom_quat[0] odom.pose.pose.orientation.y = odom_quat[1] odom.pose.pose.orientation.z = odom_quat[2] odom.pose.pose.orientation.w = odom_quat[3] # set the velocity odom.twist.twist.linear.x = self.vx odom.twist.twist.linear.y = self.vy odom.twist.twist.angular.z = self.vth # publish the message self.odom_pub.publish(odom)
def _broadcastTransform(self, Pose, parentName, childName): br = TransformBroadcaster() tr = TransformStamped() tr.header.stamp = Time.now() tr.header.frame_id = parentName tr.child_frame_id = childName tr.transform.translation = Pose.position tr.transform.rotation = Pose.orientation br.sendTransform(tr)
def __init__(self): super().__init__('auv_transform_publisher') qos_profile = QoSProfile(depth=10) self.state_subscriber = self.create_subscription( PoseStamped, '/triton/state', self.state_callback, 10) self.path_publisher = self.create_publisher(Path, '/triton/path', qos_profile) self.pose_array = [] self.broadcaster = TransformBroadcaster(self, qos=qos_profile) self.get_logger().info("AUVTransformPublisher successfully started!")
def __init__(self): ############################################################################# super().__init__("odometry_encoders") self.nodename = self.get_name() self.get_logger().info("%s started" % self.nodename) #### parameters ####### self.radius = float( self.declare_parameter( 'wheels.radius', 0.02569).value) # The wheel radius in meters self.base_width = float( self.declare_parameter( 'wheels.base_width', 0.1275).value) # The wheel base width in meters # the name of the base frame of the robot self.base_frame_id = self.declare_parameter('base_frame_id', 'base_link').value # the name of the odometry reference frame self.odom_frame_id = self.declare_parameter('odom_frame_id', 'odom').value self.ticks_mode = self.declare_parameter('ticks.ticks_mode', False).value # The number of wheel encoder ticks per meter of travel self.ticks_meter = float( self.declare_parameter('ticks.ticks_meter', 50.0).value) self.encoder_min = self.declare_parameter('ticks.encoder_min', -32768).value self.encoder_max = self.declare_parameter('ticks.encoder_max', 32768).value # Init variables self.init() # subscriptions / publishers self.create_subscription(Wheels, "robot/enc_wheels", self.wheelsCallback, qos_profile_system_default) self.create_subscription(Wheels, "robot/enc_ticks_wheels", self.wheelsEncCallback, qos_profile_system_default) self.cal_vel_pub = self.create_publisher(Twist, "cal_vel", qos_profile_system_default) self.odom_pub = self.create_publisher(Odometry, "odom", qos_profile_system_default) # 5 seconds timer to update parameters self.create_timer(5, self.parametersCallback) # TF2 Broadcaster self.odomBroadcaster = TransformBroadcaster(self)
def __init__(self): self.fixed_frame = rospy.get_param( '~fixed_frame', "world" ) # set fixed frame relative to world to apply the transform self.publish_transform = rospy.get_param( '~publish_transform', False) # set true in launch file if the transform is needed self.pub_jnt = rospy.Publisher("joint_states", JointState, queue_size=10) # joint state publisher self.odom_broadcaster = TransformBroadcaster() self.jnt_msg = JointState() # joint topic structure self.odom_trans = TransformStamped() self.odom_trans.header.frame_id = 'world' self.odom_trans.child_frame_id = 'base_link' self.jnt_msg.header = Header() self.jnt_msg.name = [ 'Rev103', 'Rev104', 'Rev105', 'Rev106', 'Rev107', 'Rev108', 'Rev109', 'Rev110', 'Rev111', 'Rev112', 'Rev113', 'Rev114', 'Rev115', 'Rev116', 'Rev117', 'Rev118', 'Rev119', 'Rev120', 'Rev121' ] """self.jnt_msg.name = ['Rev105', 'Rev111', 'Rev117', 'Rev104', 'Rev110', 'Rev116', 'Rev103', 'Rev109', 'Rev115', 'Rev108', 'Rev114', 'Rev120', 'Rev107', 'Rev113', 'Rev119', 'Rev106', 'Rev112', 'Rev118', 'Rev121']""" self.jnt_msg.velocity = [] self.jnt_msg.effort = [] rospy.on_shutdown(self.shutdown_node) rospy.loginfo("Ready for publishing") rate = rospy.Rate(50) while not rospy.is_shutdown(): self.odom_trans.header.stamp = rospy.Time.now() self.odom_trans.transform.translation.x = 0 self.odom_trans.transform.translation.y = 0 self.odom_trans.transform.translation.z = 0 self.odom_trans.transform.rotation = euler_to_quaternion( 0, 0, 0) # roll,pitch,yaw self.odom_broadcaster.sendTransform(self.odom_trans) self.publish_jnt() rate.sleep()
def callback(msg): br = TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "odom" t.child_frame_id = "base_link" t.transform.translation.x = msg.pose.pose.position.x t.transform.translation.y = msg.pose.pose.position.y t.transform.translation.z = 0.0 t.transform.rotation.x = msg.pose.pose.orientation.x t.transform.rotation.y = msg.pose.pose.orientation.y t.transform.rotation.z = msg.pose.pose.orientation.z t.transform.rotation.w = msg.pose.pose.orientation.w br.sendTransform(t)
def __init__(self): """ Start up connection to the Neato Robot. """ rospy.init_node('neato') self.port = rospy.get_param('~port', "/dev/ttyUSB0") rospy.loginfo("Using port: %s" % (self.port)) self.robot = xv11(self.port) rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb) self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10) self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10) self.odomBroadcaster = TransformBroadcaster() self.cmd_vel = [0, 0]
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, 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, node_name): super(DeadReckoningNode, self).__init__( node_name=node_name, node_type=NodeType.LOCALIZATION) self.node_name = node_name self.veh = rospy.get_param('~veh') self.publish_hz = rospy.get_param('~publish_hz') self.encoder_stale_dt = rospy.get_param('~encoder_stale_dt') self.ticks_per_meter = rospy.get_param('~ticks_per_meter') self.wheelbase = rospy.get_param('~wheelbase') self.origin_frame = rospy.get_param('~origin_frame').replace('~', self.veh) self.target_frame = rospy.get_param('~target_frame').replace('~', self.veh) self.debug = rospy.get_param('~debug', False) self.left_encoder_last = None self.right_encoder_last = None self.encoders_timestamp_last = None self.encoders_timestamp_last_local = None # Current pose, forward velocity, and angular rate self.timestamp = None self.x = 0.0 self.y = 0.0 self.z = 0.0 self.yaw = 0.0 self.q = [0.0, 0.0, 0.0, 1.0] self.tv = 0.0 self.rv = 0.0 # Used for debugging self.x_trajectory = [] self.y_trajectory = [] self.yaw_trajectory = [] self.time = [] self.total_dist = 0 # Setup subscribers self.sub_encoder_left = message_filters.Subscriber("~left_wheel", WheelEncoderStamped) self.sub_encoder_right = message_filters.Subscriber("~right_wheel", WheelEncoderStamped) # Setup the time synchronizer self.ts_encoders = message_filters.ApproximateTimeSynchronizer( [self.sub_encoder_left, self.sub_encoder_right], 10, 0.5) self.ts_encoders.registerCallback(self.cb_ts_encoders) # Setup publishers self.pub = rospy.Publisher('~odom', Odometry, queue_size=10) # Setup timer self.timer = rospy.Timer(rospy.Duration(1 / self.publish_hz), self.cb_timer) self._print_time = 0 self._print_every_sec = 30 # tf broadcaster for odometry TF self._tf_broadcaster = TransformBroadcaster() self.loginfo("Initialized")
class TfWrapper(object): 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(.05) self.broadcasting_frames = {} self.broadcasting_frames_lock = Lock() rospy.sleep(0.1) def transform_pose(self, target_frame, pose): try: transform = self.tfBuffer.lookup_transform( target_frame, pose.header.frame_id, # source frame pose.header.stamp, rospy.Duration(2.0)) new_pose = do_transform_pose(pose, transform) return new_pose except ExtrapolationException as e: rospy.logwarn(e) def lookup_transform(self, target_frame, source_frame): p = PoseStamped() p.header.frame_id = source_frame p.pose.orientation.w = 1.0 return self.transform_pose(target_frame, p) def set_frame_from_pose(self, name, pose_stamped): with self.broadcasting_frames_lock: frame = TransformStamped() frame.header = pose_stamped.header frame.child_frame_id = name frame.transform.translation = pose_stamped.pose.position frame.transform.rotation = pose_stamped.pose.orientation self.broadcasting_frames[name] = frame def start_frame_broadcasting(self): self.tf_broadcaster = TransformBroadcaster() self.tf_timer = rospy.Timer(self.tf_frequency, self.broadcasting_cb) def broadcasting_cb(self, _): with self.broadcasting_frames_lock: for frame in self.broadcasting_frames.values(): frame.header.stamp = rospy.get_rostime() self.tf_broadcaster.sendTransform(frame)
class TfBridge(object): """ Utility class to interface with /tf2 """ def __init__(self): """ """ self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.tf_broadcaster = TransformBroadcaster() def publish_pose_to_tf(self, pose, source_frame, target_frame, time=None): """ Publish the given pose to /tf2 """ msg = pose.to_msg() transform = TransformStamped() transform.child_frame_id = target_frame transform.header.frame_id = source_frame if time is not None: transform.header.stamp = time else: transform.header.stamp = rospy.Time().now() transform.transform.translation = msg.position transform.transform.rotation = msg.orientation self.tf_broadcaster.sendTransform(transform) def get_pose_from_tf(self, source_frame, target_frame, time=None): """ Get the pose from /tf2 """ try: if time is not None: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, time) else: trans = self.tf_buffer.lookup_transform( source_frame, target_frame, rospy.Time(0)) x = trans.transform.translation.x y = trans.transform.translation.y z = trans.transform.translation.z rx = trans.transform.rotation.x ry = trans.transform.rotation.y rz = trans.transform.rotation.z rw = trans.transform.rotation.w pose = Vector6D(x=x, y=y, z=z).from_quaternion(rx, ry, rz, rw) return True, pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logwarn("[perception] Exception occured: {}".format(e)) return False, None
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)
def handle_turtle_pose(msg, turtlename): transform_broadcaster = TransformBroadcaster() transformed_msg = TransformStamped() transformed_msg.header.stamp = rospy.Time.now() transformed_msg.header.frame_id = 'world' transformed_msg.child_frame_id = turtlename transformed_msg.transform.translation.x = msg.x transformed_msg.transform.translation.y = msg.y transformed_msg.transform.translation.z = 0.0 q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta) transformed_msg.transform.rotation.x = q[0] transformed_msg.transform.rotation.y = q[1] transformed_msg.transform.rotation.z = q[2] transformed_msg.transform.rotation.w = q[3] transform_broadcaster.sendTransform(transformed_msg)
def __init__(self): rclpy.init() super().__init__('diff_drive_odometry') self.odometry = odometry.Odometry() qos_profile = QoSProfile(depth=10) self.odomPub = self.create_publisher(Odometry, 'odom', qos_profile) self.tfPub = TransformBroadcaster(self, qos=qos_profile) self.nodeName = self.get_name() self.get_logger().info("{0} started".format(self.nodeName)) self.create_subscription(Int32, "lwheel_ticks", self.leftCallback, qos_profile) self.create_subscription(Int32, "rwheel_ticks", self.rightCallback, qos_profile) self.create_subscription(PoseWithCovarianceStamped, "initialpose", self.on_initial_pose, qos_profile) self.declare_parameters(namespace='', parameters=[('ticks_per_meter', 1000), ('wheel_separation', 100), ('rate', 10.0), ('base_frame_id', 'base_link'), ('odom_frame_id', 'odom'), ('encoder_min', -32768), ('encoder_max', 32767)]) self.ticksPerMeter = int(self.get_parameter('ticks_per_meter').value) self.wheelSeparation = float( self.get_parameter('wheel_separation').value) self.rate = float(self.get_parameter('rate').value) self.baseFrameID = self.get_parameter('base_frame_id').value self.odomFrameID = self.get_parameter('odom_frame_id').value self.encoderMin = int(self.get_parameter('encoder_min').value) self.encoderMax = int(self.get_parameter('encoder_max').value) self.odometry.setWheelSeparation(self.wheelSeparation) self.odometry.setTicksPerMeter(self.ticksPerMeter) self.odometry.setEncoderRange(self.encoderMin, self.encoderMax) self.odometry.setTime(self.get_clock().now()) rate = self.create_rate(self.rate) while rclpy.ok(): self.publish() rate.sleep()
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)
class Pose2Tf(): def __init__(self): self.tf_broadcaster = TransformBroadcaster() self.ts = TransformStamped() self.ts.header.frame_id = 'world' self.ts.child_frame_id = 'drone' pose_sub = Subscriber('/dji_sdk/pose', PoseStamped, self.pose_callback) def pose_callback(self, msg): self.ts.header.stamp = Time.now() self.ts.transform.translation.x = msg.pose.position.x self.ts.transform.translation.y = msg.pose.position.y self.ts.transform.translation.z = msg.pose.position.z self.ts.transform.rotation.x = msg.pose.orientation.x self.ts.transform.rotation.y = msg.pose.orientation.y self.ts.transform.rotation.z = msg.pose.orientation.z self.ts.transform.rotation.w = msg.pose.orientation.w self.tf_broadcaster.sendTransform(self.ts)
def rosSetup(self): ''' Creates and inits ROS components ''' if self.ros_initialized: return 0 self._transform_listener = TransformListener() self._transform_broadcaster = TransformBroadcaster() # Publishers self._state_publisher = rospy.Publisher('~state', MarkerMappingState, queue_size=10) self._initialpose_publisher = rospy.Publisher( '/initialpose', PoseWithCovarianceStamped, queue_size=10) self._global_pose_publisher = rospy.Publisher('/global_pose', Pose2D, queue_size=10) # Subscribers # topic_name, msg type, callback, queue_size self.markers_subscriber = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.markersCb, queue_size=10) # Service Servers self.save_marker_service_server = rospy.Service( '~save_maker', SaveMarker, self.saveMarkerServiceCb) self.init_pose_from_marker_service_server = rospy.Service( '~init_pose_from_marker', InitPoseFromMarker, self.initPoseFromMarkerServiceCb) # Service Clients # self.service_client = rospy.ServiceProxy('service_name', ServiceMsg) # ret = self.service_client.call(ServiceMsg) self.ros_initialized = True self.publishROSstate() return 0
def __init__(self): super().__init__('chassis') self.odom_pub = self.create_publisher(Odometry, "odom_dirty", 10) self.odom_broadcaster = TransformBroadcaster(self) self.x = 0 self.y = 0 self.yaw = 0 self.Vx = 0 self.yawRate = 0 self.cmdSpeed = 0 self.cmdSteering = 0 self.tsFeedback = self.get_clock().now().nanoseconds self.chassis = ChassisInterface() self.chassis.setWheelCallback(self.processWheelFeedback) self.sub = self.create_subscription(Twist, 'cmd_vel', self.processCmd, 10) logging.info('chassis node initialized')
def run(self): rospy.init_node('ackmn_simulator', anonymous=True) rospy.Subscriber("/ackmn_drive", AckermannDrive, self.ackmn_callback) self.pub_twist = rospy.Publisher("/twist_sim", Twist, queue_size=1) self.br = TransformBroadcaster() r = rospy.Rate(50) self.t0 = rospy.get_rostime() while not rospy.is_shutdown(): self.sim() self.publish_msgs() r.sleep()
class WheelOdom(Node): def __init__(self): super().__init__('wheel_odom') self.create_timer(0.1, self.cb) self.odom_broadcaster = TransformBroadcaster(self) print('fake odom') def cb(self): now = self.get_clock().now() quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = 0.0 quaternion.w = 1.0 transform_stamped_msg = TransformStamped() transform_stamped_msg.header.stamp = now.to_msg() transform_stamped_msg.header.frame_id = 'odom' transform_stamped_msg.child_frame_id = 'base_link' transform_stamped_msg.transform.translation.x = 0.0 transform_stamped_msg.transform.translation.y = 0.0 transform_stamped_msg.transform.translation.z = 0.0 transform_stamped_msg.transform.rotation.x = quaternion.x transform_stamped_msg.transform.rotation.y = quaternion.y transform_stamped_msg.transform.rotation.z = quaternion.z transform_stamped_msg.transform.rotation.w = quaternion.w self.odom_broadcaster.sendTransform(transform_stamped_msg) quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = 0.0 quaternion.w = 1.0 transform_stamped_msg = TransformStamped() transform_stamped_msg.header.stamp = now.to_msg() transform_stamped_msg.header.frame_id = 'map' transform_stamped_msg.child_frame_id = 'odom' transform_stamped_msg.transform.translation.x = 0.0 transform_stamped_msg.transform.translation.y = 0.0 transform_stamped_msg.transform.translation.z = 0.0 transform_stamped_msg.transform.rotation.x = quaternion.x transform_stamped_msg.transform.rotation.y = quaternion.y transform_stamped_msg.transform.rotation.z = quaternion.z transform_stamped_msg.transform.rotation.w = quaternion.w self.odom_broadcaster.sendTransform(transform_stamped_msg) quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = 0.0 quaternion.w = 1.0 transform_stamped_msg = TransformStamped() transform_stamped_msg.header.stamp = now.to_msg() transform_stamped_msg.header.frame_id = 'base_link' transform_stamped_msg.child_frame_id = 'camera_link' transform_stamped_msg.transform.translation.x = 0.0 transform_stamped_msg.transform.translation.y = 0.0 transform_stamped_msg.transform.translation.z = 0.4 transform_stamped_msg.transform.rotation.x = quaternion.x transform_stamped_msg.transform.rotation.y = quaternion.y transform_stamped_msg.transform.rotation.z = quaternion.z transform_stamped_msg.transform.rotation.w = quaternion.w self.odom_broadcaster.sendTransform(transform_stamped_msg)
def __init__(self, config, visualize=True, verbose=True, name="one"): self.visualize = visualize self.verbose = verbose # parameters for simulation self.drive_topic = rospy.get_param("~drive_topic_%s" % name) self.scan_topic = rospy.get_param("~scan_topic_%s" % name) self.pose_topic = rospy.get_param("~pose_topic_%s" % name) self.odom_topic = rospy.get_param("~odom_topic_%s" % name) self.map_frame = rospy.get_param("~map_frame") self.base_frame = rospy.get_param("~base_frame") self.scan_frame = rospy.get_param("~scan_frame") self.update_pose_rate = rospy.get_param("~update_pose_rate") self.config = config # racecar object self.rcs = RacecarSimulator(config) # transform broadcaster self.br = TransformBroadcaster() # publishers self.scan_pub = rospy.Publisher(self.scan_topic, LaserScan) self.odom_pub = rospy.Publisher(self.odom_topic, Odometry) # subscribers self.drive_sub = rospy.Subscriber(self.drive_topic, AckermannDriveStamped, self.driveCallback, queue_size=1) self.pose_sub = rospy.Subscriber(self.pose_topic, PoseStamped, self.poseCallback) if self.verbose: print "Player %s construted!" % name
def __init__(self): rclpy.init() super().__init__('oakd_publisher') qos_profile = QoSProfile(depth=10) self.broadcaster = TransformBroadcaster(self, qos=qos_profile) #Image publisher #self.pub_depth_img = self.create_publisher(sensor_msgs.msg.Image, "/depth_img", 10) self.pub_rectified_img = self.create_publisher(Image, "/rectified_img", 10) # Transform for head declaration self.transform_head = TransformStamped() self.transform_head.header.frame_id = 'oak-d_camera_center' self.transform_head.child_frame_id = 'head' # Transform for palm declaration self.transform_palm = TransformStamped() self.transform_palm.header.frame_id = 'oak-d_camera_center' self.transform_palm.child_frame_id = 'palm' #fixed Identity quaternion (no rotation) self.fps = FPS() self.advanced_main()
def __init__(self): rclpy.init() super().__init__('state_publisher') qos_profile = QoSProfile(depth=10) self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile) self.broadcaster = TransformBroadcaster(self, qos=qos_profile) self.nodeName = self.get_name() self.get_logger().info("{0} started".format(self.nodeName)) loop_rate = self.create_rate(30) # robot state steering_angle = 0. wheel_angle = 0. # message declarations odom_trans = TransformStamped() odom_trans.header.frame_id = 'odom' odom_trans.child_frame_id = 'base_link' joint_state = JointState() try: t = 0 while rclpy.ok(): t += 1. / 30. self.get_logger().info(str(t)) rclpy.spin_once(self) # Set angle rotation_names = [ 'front_left_wheel_joint', 'front_right_wheel_joint', 'rear_right_wheel_joint', 'rear_left_wheel_joint' ] wheel_angle += 2 * pi * t / 50 wheel_angle = atan2(sin(wheel_angle), cos(wheel_angle)) rotation_position = [ wheel_angle, wheel_angle, wheel_angle, wheel_angle ] # Set steering steering_angle = pi / 3. * sin(t * 2 * pi / 4.) steering_names = [ 'front_left_steering_joint', 'front_right_steering_joint' ] steering_position = [steering_angle, steering_angle] # update joint_state now = self.get_clock().now() joint_state.header.stamp = now.to_msg() joint_state.name = rotation_names + steering_names joint_state.position = rotation_position + steering_position # update transform # (moving in a circle with radius=2) angle = t * 2 * pi / 10. odom_trans.header.stamp = now.to_msg() odom_trans.transform.translation.x = cos(angle) * 2 odom_trans.transform.translation.y = sin(angle) * 2 odom_trans.transform.translation.z = 0.7 odom_trans.transform.rotation = \ euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw # send the joint state and transform self.joint_pub.publish(joint_state) self.broadcaster.sendTransform(odom_trans) # This will adjust as needed per iteration loop_rate.sleep() except KeyboardInterrupt: pass
class ChassisNode(Node): CameraElevationMeters = 0.4 WheelBaselineMeters = 0.430 WheelRadiusMeters = 0.115 ReportIntervalSec = 0.02 SecInNsec = 10**-9 RadPerSecInWheelFeedback = 1. MaxSpeedMetersSec = 1.0 # linear velocity from vel_cmd is scaled and bounded to this MaxYawRateRadSec = 1.0 # yaw rate from vel_cmd is scaled and bounded to this. todo : set to actual max yaw rate of chassis def __init__(self): super().__init__('chassis') self.odom_pub = self.create_publisher(Odometry, "odom_dirty", 10) self.odom_broadcaster = TransformBroadcaster(self) self.x = 0 self.y = 0 self.yaw = 0 self.Vx = 0 self.yawRate = 0 self.cmdSpeed = 0 self.cmdSteering = 0 self.tsFeedback = self.get_clock().now().nanoseconds self.chassis = ChassisInterface() self.chassis.setWheelCallback(self.processWheelFeedback) self.sub = self.create_subscription(Twist, 'cmd_vel', self.processCmd, 10) logging.info('chassis node initialized') def processWheelFeedback(self, DeviceIdIgnored, respTuple): now = self.get_clock().now() deltaTimeSec = (now.nanoseconds - self.tsFeedback) * ChassisNode.SecInNsec zeroRotation = Quaternion() zeroTranslation = Vector3() zeroTranslation.x = 0.0 zeroTranslation.y = 0.0 zeroTranslation.z = 0.0 if deltaTimeSec >= ChassisNode.ReportIntervalSec: self.tsFeedback = now.nanoseconds wheelLRadSec = respTuple[0] * ChassisNode.RadPerSecInWheelFeedback wheelRRadSec = respTuple[1] * ChassisNode.RadPerSecInWheelFeedback self.calculatePose(deltaTimeSec, wheelRRadSec, wheelLRadSec) logging.debug( 'chassis wheel feedback : {0} {1} speed {2} yawRate {3} pos [{4} {5}] yaw {6}' .format(wheelLRadSec, wheelRRadSec, self.Vx, self.yawRate, self.x, self.y, self.yaw)) quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(self.yaw / 2) quaternion.w = cos(self.yaw / 2) odom = Odometry() odom.header.stamp = now.to_msg() odom.header.frame_id = 'odom' odom.child_frame_id = 'base_link' odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0.0 odom.pose.covariance[0] = 0.1 odom.pose.covariance[7] = 0.1 odom.pose.covariance[35] = 0.1 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = self.Vx odom.twist.twist.linear.y = 0.0 odom.twist.twist.angular.z = self.yawRate odom.twist.covariance[0] = 0.01 odom.twist.covariance[7] = 0.01 odom.twist.covariance[35] = 0.01 self.odom_pub.publish(odom) transform_stamped_msg = TransformStamped() transform_stamped_msg.header.stamp = now.to_msg() transform_stamped_msg.header.frame_id = 'odom' transform_stamped_msg.child_frame_id = 'base_link' transform_stamped_msg.transform.translation = zeroTranslation transform_stamped_msg.transform.translation.x = self.x transform_stamped_msg.transform.translation.y = self.y transform_stamped_msg.transform.rotation = quaternion self.odom_broadcaster.sendTransform(transform_stamped_msg) transform_stamped_msg = TransformStamped() transform_stamped_msg.header.stamp = now.to_msg() transform_stamped_msg.header.frame_id = 'map' transform_stamped_msg.child_frame_id = 'odom' transform_stamped_msg.transform.translation = zeroTranslation transform_stamped_msg.transform.rotation = zeroRotation self.odom_broadcaster.sendTransform(transform_stamped_msg) transform_stamped_msg = TransformStamped() transform_stamped_msg.header.stamp = now.to_msg() transform_stamped_msg.header.frame_id = 'base_link' transform_stamped_msg.child_frame_id = 'camera_link' transform_stamped_msg.transform.translation = zeroTranslation transform_stamped_msg.transform.translation.z = ChassisNode.CameraElevationMeters self.odom_broadcaster.sendTransform(transform_stamped_msg) def processCmd(self, data): #todo : add deadzone for Vx and yaw rate #todo : reset chassis control to zeroes if cmd_vel has expired cmdSpeedBounded = max(min(data.linear.x, MaxSpeedMetersSec), -MaxSpeedMetersSec) self.cmdSpeed = cmdSpeedBounded / MaxSpeedMetersSec self.chassis.setSpeed(self.cmdSpeed) cmdYawRateBounded = max(min(data.angular.z, MaxYawRateRadSec), -MaxYawRateRadSec) self.cmdSteering = cmdYawRateBounded / MaxYawRateRadSec self.chassis.setSteering(self.cmdSteering) logging.debug('chassis cmd speed {0}/{1} steering {2}/{3}'.format( data.linear.x, self.cmdSpeed, data.angular.z, cmdSteering)) #dtSec : integration interval #Lvel, Rvel : wheels angular speed, rad/sec def calculatePose(self, dtSec, Lvel, Rvel): # Lvel= -Lvel self.Vx = (ChassisNode.WheelRadiusMeters) * (Rvel + Lvel) / 2 self.yawRate = -1.6 * (ChassisNode.WheelRadiusMeters) * ( Rvel - Lvel) / ChassisNode.WheelBaselineMeters self.yaw += dtSec * self.yawRate self.y += dtSec * sin(self.yaw) * self.Vx self.x += dtSec * cos(self.yaw) * self.Vx
def read_raw(self): if self.__count > 0: self.__data = np.multiply(self.__temp, 1.0 / self.__count) norm = np.linalg.norm(self.__data) self.__data /= norm self.__count = 0 self.__temp = np.zeros((4, )) return self.__data def setOffset(self, offset): offset[3] = -offset[3] self.__offset_inv = np.array(offset, dtype=np.float64) self.__q_offset = quaternion_multiply(self.__ref, self.__offset_inv) br = TransformBroadcaster() def sendTF(frame_id, child_frame_id, xyz=[0, 0, 0], q=[0, 0, 0, 1]): tf = TransformStamped() tf.header.stamp = rospy.Time.now() tf.header.frame_id = frame_id tf.child_frame_id = child_frame_id tf.transform.translation = Vector3(xyz[0], xyz[1], xyz[2]) tf.transform.rotation = Quaternion(q[0], q[1], q[2], q[3]) br.sendTransform(tf) JR1 = IMU(ref=quaternion_from_euler(0, math.pi / 2, 0)) JR2 = IMU(ref=quaternion_from_euler(math.pi / 2, 0, math.pi / 2)) JR3 = IMU(ref=quaternion_from_euler(math.pi / 2, 0, math.pi / 2))