def __init__(self): # Networking parameters: self.client_ip = rospy.get_param("~client_ip", "127.0.0.1") self.self_ip = rospy.get_param("~self_ip", "127.0.0.1") self.position_port = rospy.get_param("~position_port", 9000) self.metadata_port = rospy.get_param("~metadata_port", 9001) self.image_port = rospy.get_param("~image_port", 9002) self.udp_port = rospy.get_param("~udp_port", 9004) self.step_port = rospy.get_param("~step_port", 9005) # Set data to publish # `publish_mono_stereo` is true to publish one channel stereo images # Otherwise, publish as bgr8 publish_mono_stereo = rospy.get_param("~publish_mono_stereo", True) publish_segmentation = rospy.get_param("~publish_segmentation", True) publish_depth = rospy.get_param("~publish_depth", True) self.publish_metadata = rospy.get_param("~publish_metadata", False) # Camera parameters: self.camera_width = rospy.get_param("~camera_width", 720) assert (self.camera_width > 0) assert (self.camera_width % 2 == 0) self.camera_height = rospy.get_param("~camera_height", 480) assert (self.camera_height > 0) assert (self.camera_height % 2 == 0) self.camera_fov = rospy.get_param("~camera_vertical_fov", 60) assert (self.camera_fov > 0) self.stereo_baseline = rospy.get_param("~stereo_baseline", 0.2) assert (self.stereo_baseline > 0) # Near and far draw distances determine Unity camera rendering bounds. self.near_draw_dist = rospy.get_param("~near_draw_dist", 0.05) self.far_draw_dist = rospy.get_param("~far_draw_dist", 50) # Simulator speed parameters: self.speedup_factor = rospy.get_param("~speedup_factor", 1.0) assert (self.speedup_factor > 0.0) # We are dividing by this so > 0 self.frame_rate = rospy.get_param("~frame_rate", 20.0) self.imu_rate = rospy.get_param("~imu_rate", 200.0) # Output parameters: self.world_frame_id = rospy.get_param("~world_frame_id", "world") self.body_frame_id = rospy.get_param("~body_frame_id", "base_link_gt") self.left_cam_frame_id = rospy.get_param("~left_cam_frame_id", "left_cam") self.right_cam_frame_id = rospy.get_param("~right_cam_frame_id", "right_cam") assert (self.left_cam_frame_id != self.right_cam_frame_id) self.env = Env(simulation_ip=self.client_ip, own_ip=self.self_ip, position_port=self.position_port, metadata_port=self.metadata_port, image_port=self.image_port, step_port=self.step_port) # To send images via ROS network and convert from/to ROS self.cv_bridge = CvBridge() # publish left and right cameras as mono8 or bgr8, depending on the given param n_stereo_channels = Channels.SINGLE if publish_mono_stereo else Channels.THREE self.cameras = [(Camera.RGB_LEFT, Compression.OFF, n_stereo_channels, self.left_cam_frame_id), (Camera.RGB_RIGHT, Compression.OFF, n_stereo_channels, self.right_cam_frame_id)] self.img_pubs = [ rospy.Publisher("left_cam/image_raw", ImageMsg, queue_size=10), rospy.Publisher("right_cam/image_raw", ImageMsg, queue_size=10) ] # setup optional publishers if publish_segmentation: self.cameras.append((Camera.SEGMENTATION, Compression.OFF, Channels.THREE, self.left_cam_frame_id)) self.img_pubs.append( rospy.Publisher("segmentation/image_raw", ImageMsg, queue_size=10)) if publish_depth: self.cameras.append((Camera.DEPTH, Compression.OFF, Channels.THREE, self.left_cam_frame_id)) self.img_pubs.append( rospy.Publisher("depth/image_raw", ImageMsg, queue_size=10)) if self.publish_metadata: self.metadata_pub = rospy.Publisher("metadata", String) # Camera information members. # TODO(marcus): reformat like img_pubs self.cam_info_pubs = [ rospy.Publisher("left_cam/camera_info", CameraInfo, queue_size=10), rospy.Publisher("right_cam/camera_info", CameraInfo, queue_size=10), rospy.Publisher("segmentation/camera_info", CameraInfo, queue_size=10), rospy.Publisher("depth/camera_info", CameraInfo, queue_size=10) ] self.cam_info_msgs = [] # If the clock updates faster than images can be queried in # step mode, the image callback is called twice on the same # timestamp which leads to duplicate published images. # Track image timestamps to prevent this self.last_image_timestamp = None # Setup ROS publishers self.imu_pub = rospy.Publisher("imu", Imu, queue_size=10) self.odom_pub = rospy.Publisher("odom", Odometry, queue_size=10) # Setup ROS services. self.setup_ros_services() # Transform broadcasters. self.tf_broadcaster = tf.TransformBroadcaster() # Don't call static_tf_broadcaster.sendTransform multiple times. # Rather call it once with multiple static tfs! Check issue #40 self.static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster() # Required states for finite difference calculations. self.prev_time = 0.0 self.prev_vel_brh = [0.0, 0.0, 0.0] self.prev_enu_R_brh = np.identity(3) # Setup camera parameters and extrinsics in the simulator per spec. self.setup_cameras() # Setup collision enable_collision = rospy.get_param("~enable_collision", 0) self.setup_collision(enable_collision) # Change scene initial_scene = rospy.get_param("~initial_scene", 1) rospy.wait_for_service('scene_change_request') self.change_scene(initial_scene) # Setup UdpListener. self.udp_listener = UdpListener(port=self.udp_port, rate=self.imu_rate) self.udp_listener.subscribe('udp_subscriber', self.udp_cb) # Simulated time requires that we constantly publish to '/clock'. self.clock_pub = rospy.Publisher("/clock", Clock, queue_size=10) # Setup simulator step mode step_mode_enabled = rospy.get_param("~enable_step_mode", False) if step_mode_enabled: self.env.send(SetFrameRate(self.frame_rate)) print("TESSE_ROS_NODE: Initialization complete.")
def __init__(self, init_trans, update_freq): rospy.Subscriber('aruco/markers', MarkerArray, self.update_callback) rospy.Subscriber('sign_pose', MarkerArray, self.update_callback) #rospy.Subscriber('marker_measurements', MarkerArray, self.update_callback) rospy.Subscriber("cf1/pose", PoseStamped, self.cf1_pose_callback) rospy.Subscriber("cf1/velocity", TwistStamped, self.cf1_vel_callback) self.cf1_pose_cov_pub = rospy.Publisher("cf1/localizatiton/pose_cov", PoseWithCovarianceStamped, queue_size=1) init_trans.header.frame_id = "map" init_trans.child_frame_id = "cf1/odom" self.init_trans = init_trans # remove? self.last_transform = init_trans self.cf1_pose = None self.cf1_vel = None self.old_msg = None self.measurement_msg = None # what translation/rotation variables to use in filtering self.filter_config = rospy.get_param("localization/measurement_config") self.tf_buf = tf2_ros.Buffer() self.tf_lstn = tf2_ros.TransformListener( self.tf_buf, queue_size=1) # should there be a queue_size? self.broadcaster = tf2_ros.TransformBroadcaster() self.static_broadcaster = tf2_ros.StaticTransformBroadcaster() self.update_freq = update_freq self.kf = KalmanFilter( initial_cov=np.diag(rospy.get_param("localization/initial_cov")), R=np.diag(rospy.get_param("localization/process_noise")), maha_dist_thres=rospy.get_param("localization/maha_dist_thres"), cov_norm_thres=rospy.get_param("localization/cov_norm_thres"), delta_t=1.0 / self.update_freq) self.maha_dist_thres = rospy.get_param("localization/maha_dist_thres") self.cov_norm_thres = rospy.get_param("localization/cov_norm_thres") self.converged_pub = rospy.Publisher("cf1/localization/converged", Bool, queue_size=1) self.measurement_fb_pub = rospy.Publisher( "cf1/localization/measurement_feedback", Int32MultiArray, queue_size=1) self.odom_new_pub = rospy.Publisher("cf1/pose/odom_new", PoseStamped, queue_size=1) self.believed_pub = rospy.Publisher("cf1/pose/believed", PoseStamped, queue_size=1) self.measured_valid_pub = rospy.Publisher("cf1/pose/measured_valid", PoseArray, queue_size=1) self.measured_invalid_pub = rospy.Publisher( "cf1/pose/measured_invalid", PoseArray, queue_size=1) self.filtered_pub = rospy.Publisher("cf1/pose/filtered", PoseStamped, queue_size=1) rospy.Service("cf1/localization/reset_kalman_filter", ResetKalmanFilter, self.reset_kalman_filter)
def start_spot_ros_interface(self): # ROS Node initialization rospy.init_node('spot_ros_interface_py') rate = rospy.Rate(200) # Update at 200 Hz # Each service will handle a specific command to Spot instance rospy.Service("self_right_cmd", spot_ros_srvs.srv.Stand, self.self_right_cmd_srv) rospy.Service("stand_cmd", spot_ros_srvs.srv.Stand, self.stand_cmd_srv) rospy.Service("trajectory_cmd", spot_ros_srvs.srv.Trajectory, self.trajectory_cmd_srv) rospy.Service("velocity_cmd", spot_ros_srvs.srv.Velocity, self.velocity_cmd_srv) # Single image publisher will publish all images from all Spot cameras kinematic_state_pub = rospy.Publisher( "kinematic_state", spot_ros_msgs.msg.KinematicState, queue_size=20) robot_state_pub = rospy.Publisher( "robot_state", spot_ros_msgs.msg.RobotState, queue_size=20) occupancy_grid_pub = rospy.Publisher( "occupancy_grid", visualization_msgs.msg.Marker, queue_size=20) # Publish tf2 from visual odometry frame to Spot's base link spot_tf_broadcaster = tf2_ros.TransformBroadcaster() # Publish static tf2 from Spot's base link to front-left camera spot_tf_static_broadcaster = tf2_ros.StaticTransformBroadcaster() image_only_pub = rospy.Publisher( "spot_image", sensor_msgs.msg.Image, queue_size=20) camera_info_pub = rospy.Publisher( "spot_cam_info", sensor_msgs.msg.CameraInfo, queue_size=20) # TODO: Publish depth images # depth_image_pub = rospy.Publisher( # "depth_image", sensor_msgs.msg.Image, queue_size=20) # For RViz 3rd person POV visualization if self.third_person_view: joint_state_pub = rospy.Publisher( "joint_state_from_spot", sensor_msgs.msg.JointState, queue_size=20) try: with bosdyn.client.lease.LeaseKeepAlive(self.lease_client), bosdyn.client.estop.EstopKeepAlive( self.estop_endpoint): rospy.loginfo("Acquired lease") if self.motors_on: rospy.loginfo("Powering on robot... This may take a several seconds.") self.robot.power_on(timeout_sec=20) assert self.robot.is_powered_on(), "Robot power on failed." rospy.loginfo("Robot powered on.") else: rospy.loginfo("Not powering on robot, continuing") while not rospy.is_shutdown(): ''' Publish Robot State''' kinematic_state, robot_state = self.get_robot_state() kinematic_state_pub.publish(kinematic_state) robot_state_pub.publish(robot_state) # Publish tf2 from the fixed vision_odometry_frame to the Spot's base_link t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "vision_odometry_frame" t.child_frame_id = "base_link" t.transform.translation.x = kinematic_state.vision_tform_body.translation.x t.transform.translation.y = kinematic_state.vision_tform_body.translation.y t.transform.translation.z = kinematic_state.vision_tform_body.translation.z t.transform.rotation.x = kinematic_state.vision_tform_body.rotation.x t.transform.rotation.y = kinematic_state.vision_tform_body.rotation.y t.transform.rotation.z = kinematic_state.vision_tform_body.rotation.z t.transform.rotation.w = kinematic_state.vision_tform_body.rotation.w spot_tf_broadcaster.sendTransform(t) if self.third_person_view: joint_state_pub.publish(kinematic_state.joint_states) ''' Publish Images''' img_reqs = [image_pb2.ImageRequest(image_source_name=source, image_format=image_pb2.Image.FORMAT_RAW) for source in self.image_source_names[2:3]] image_list = self.image_client.get_image(img_reqs) for img in image_list: if img.status == image_pb2.ImageResponse.STATUS_OK: header = std_msgs.msg.Header() header.stamp = t.header.stamp header.frame_id = img.source.name if img.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: dtype = np.uint16 else: dtype = np.uint8 if img.shot.image.format == image_pb2.Image.FORMAT_RAW: image = np.fromstring(img.shot.image.data, dtype=dtype) image = image.reshape(img.shot.image.rows, img.shot.image.cols) # Make Image component of ImageCapture i = sensor_msgs.msg.Image() i.header = header i.width = img.shot.image.cols i.height = img.shot.image.rows i.data = img.shot.image.data if img.shot.image.format != image_pb2.Image.FORMAT_RAW else image.tobytes() i.step = img.shot.image.cols i.encoding = 'mono8' # CameraInfo cam_info = sensor_msgs.msg.CameraInfo() cam_info.header = i.header cam_info.width = i.width cam_info.height = i.height cam_info.distortion_model = "plumb_bob" cam_info.D = [0.0,0.0,0.0,0.0] f = img.source.pinhole.intrinsics.focal_length c = img.source.pinhole.intrinsics.principal_point cam_info.K = \ [f.x, 0, c.x, \ 0, f.y, c.y, \ 0, 0, 1] # Transform from base_link to camera for current img body_tform_cam = get_a_tform_b(img.shot.transforms_snapshot, BODY_FRAME_NAME, img.shot.frame_name_image_sensor) # Generate camera to body Transform body_tform_cam_tf = geometry_msgs.msg.Transform() body_tform_cam_tf.translation.x = body_tform_cam.position.x body_tform_cam_tf.translation.y = body_tform_cam.position.y body_tform_cam_tf.translation.z = body_tform_cam.position.z body_tform_cam_tf.rotation.x = body_tform_cam.rotation.x body_tform_cam_tf.rotation.y = body_tform_cam.rotation.y body_tform_cam_tf.rotation.z = body_tform_cam.rotation.z body_tform_cam_tf.rotation.w = body_tform_cam.rotation.w camera_transform_stamped = geometry_msgs.msg.TransformStamped() camera_transform_stamped.header.stamp = header.stamp camera_transform_stamped.header.frame_id = "base_link" camera_transform_stamped.transform = body_tform_cam_tf camera_transform_stamped.child_frame_id = img.source.name # Publish body to camera static tf spot_tf_static_broadcaster.sendTransform(camera_transform_stamped) # Publish current image and camera info image_only_pub.publish(i) camera_info_pub.publish(cam_info) ''' Publish occupancy grid''' if occupancy_grid_pub.get_num_connections() > 0: local_grid_proto = self.grid_client.get_local_grids(['terrain']) markers = get_terrain_markers(local_grid_proto) occupancy_grid_pub.publish(markers) rospy.logdebug("Looping...") rate.sleep() finally: # If we successfully acquired a lease, return it. self.lease_client.return_lease(self.lease)
def callback(data): global rel global tf_listener global debug rate = rospy.Rate(10.0) try: (trans_camera_map, rot_camera_map) = tf_listener.lookupTransform( '/head_rgbd_sensor_rgb_frame', '/map', rospy.Time(0)) # tf from camera frame to map frame tf_camera_map = KDLFrame( np.concatenate((trans_camera_map, rot_camera_map), axis=None)) # print(tf_tag0) # print(trans) for i in range(len(data.detections)): # print(data.detections[i].id, data.header.seq) # if data.detections[i].id == (0,): marker_id = data.detections[i].id # rel_id = next(x for x in rel if x.get('object_id') == marker_id) try: if debug: print(marker_id[0]) # rel_id = [i for i,val in rel if val.get('object_id')==marker_id[0]] rel_id = next(i for i, x in enumerate(rel) if x.get('object_id') == marker_id[0]) if debug: print(rel_id) except: print("problem in rel_id") break if debug: print(rel[rel_id]) rel_p = rel[rel_id].get('marker')[0].get('translation') rel_o = rel[rel_id].get('marker')[0].get('rotation') tf_relative = KDLFrame(rel_p + rel_o) # print(data.detections[i].pose.pose.pose.position) # position and orientation of tag p_0 = data.detections[i].pose.pose.pose.position o_0 = data.detections[i].pose.pose.pose.orientation # tf from camera to object # orientation of apriltags is not great. set to (1,0,0,0) frame orientation # tf_cam_tag_0 = KDLFrame([p_0.x,p_0.y,p_0.z,o_0.x,o_0.y,o_0.z,o_0.w]) tf_cam_tag_0 = KDLFrame( [p_0.x, p_0.y, p_0.z, o_0.x, o_0.y, o_0.z, o_0.w]) # x,y,z,tag_r, tag_p, tag_y = tf_cam_tag_0.get_translation_and_rpy() # print(tag_r) # print(tag_p) # print(tag_y) # # tag_r = -pi # tag_p = 0.0 # tag_y = 0.0 # tf_cam_tag_0 = KDLFrame([x,y,z]+tf.transformations.quaternion_from_euler(tag_r,tag_p,tag_y).tolist()) tf_map_tag_0 = tf_camera_map.inverse() * tf_cam_tag_0 tf_map_tag_0.get_translation_and_rpy() object_location = tf_map_tag_0 * tf_relative x, y, z, tag_r, tag_p, tag_y = object_location.get_translation_and_rpy( ) if debug: print(tag_r) print(tag_p) print(tag_y) # object_location = tf_map_tag_0 broadcaster = tf2_ros.StaticTransformBroadcaster() static_transformStamped = geometry_msgs.msg.TransformStamped() static_transformStamped.header.stamp = rospy.Time.now() static_transformStamped.header.frame_id = "map" static_transformStamped.child_frame_id = rel[rel_id].get('name') static_transformStamped.transform.translation.x = object_location.get_translation( ) static_transformStamped.transform.rotation.w = object_location.get_quaternion( ) static_transformStamped.transform.translation.x = float( object_location.get_translation()[0]) static_transformStamped.transform.translation.y = float( object_location.get_translation()[1]) static_transformStamped.transform.translation.z = float( object_location.get_translation()[2]) static_transformStamped.transform.rotation.x = object_location.get_quaternion( )[0] static_transformStamped.transform.rotation.y = object_location.get_quaternion( )[1] static_transformStamped.transform.rotation.z = object_location.get_quaternion( )[2] static_transformStamped.transform.rotation.w = object_location.get_quaternion( )[3] broadcaster.sendTransform(static_transformStamped) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): if debug: print("nothing detected") pass
def __init__(self): rospy.init_node('turtlebot_mapping') ## Use simulation time (i.e. get time from rostopic /clock) rospy.set_param('use_sim_time', 'true') rate = rospy.Rate(10) while rospy.Time.now() == rospy.Time(0): rate.sleep() ## Get transformation of camera frame with respect to the base frame self.tfBuffer = tf2_ros.Buffer() self.tfListener = tf2_ros.TransformListener(self.tfBuffer) while True: try: # notably camera_link and not camera_depth_frame below, not sure why self.raw_base_to_camera = self.tfBuffer.lookup_transform( "base_footprint", "base_scan", rospy.Time()).transform break except tf2_ros.LookupException: rate.sleep() rotation = self.raw_base_to_camera.rotation translation = self.raw_base_to_camera.translation tf_theta = get_yaw_from_quaternion(rotation) self.base_to_camera = [translation.x, translation.y, tf_theta] ## Colocate the `ground_truth` and `base_footprint` frames for visualization purposes tf2_ros.StaticTransformBroadcaster().sendTransform( create_transform_msg((0, 0, 0), (0, 0, 0, 1), "ground_truth", "base_footprint")) ## Initial state for EKF self.EKF = None self.EKF_time = None self.current_control = np.zeros(2) self.latest_pose = None self.latest_pose_time = None self.controls = deque() self.scans = deque() N_map_lines = ArenaParams.shape[1] self.x0_map = ArenaParams.T.flatten() self.x0_map[4:] = self.x0_map[4:] + np.vstack(( NoiseParams["std_alpha"] * np.random.randn(N_map_lines - 2), # first two lines fixed NoiseParams["std_r"] * np.random.randn(N_map_lines - 2))).T.flatten() self.P0_map = np.diag( np.concatenate( (np.zeros(4), np.array([[ NoiseParams["std_alpha"]**2 for i in range(N_map_lines - 2) ], [NoiseParams["std_r"]**2 for i in range(N_map_lines - 2)]]).T.flatten()))) ## Set up publishers and subscribers self.tfBroadcaster = tf2_ros.TransformBroadcaster() rospy.Subscriber('/scan', LaserScan, self.scan_callback) rospy.Subscriber('/cmd_vel', Twist, self.control_callback) rospy.Subscriber('/gazebo/model_states', ModelStates, self.state_callback) self.ground_truth_ct = 0 self.ground_truth_map_pub = rospy.Publisher("ground_truth_map", Marker, queue_size=10) self.ground_truth_map_marker = Marker() self.ground_truth_map_marker.header.frame_id = "/world" self.ground_truth_map_marker.header.stamp = rospy.Time.now() self.ground_truth_map_marker.ns = "ground_truth" self.ground_truth_map_marker.type = 5 # line list self.ground_truth_map_marker.pose.orientation.w = 1.0 self.ground_truth_map_marker.scale.x = .025 self.ground_truth_map_marker.scale.y = .025 self.ground_truth_map_marker.scale.z = .025 self.ground_truth_map_marker.color.r = 0.0 self.ground_truth_map_marker.color.g = 1.0 self.ground_truth_map_marker.color.b = 0.0 self.ground_truth_map_marker.color.a = 1.0 self.ground_truth_map_marker.lifetime = rospy.Duration(1000) self.ground_truth_map_marker.points = sum( [[Point(p1[0], p1[1], 0), Point(p2[0], p2[1], 0)] for p1, p2 in ARENA], []) self.EKF_map_pub = rospy.Publisher("EKF_map", Marker, queue_size=10) self.EKF_map_marker = deepcopy(self.ground_truth_map_marker) self.EKF_map_marker.color.r = 1.0 self.EKF_map_marker.color.g = 0.0 self.EKF_map_marker.color.b = 0.0 self.EKF_map_marker.color.a = 1.0
def __init__( self, frame, base=None, publish_pose=False, publish_twist=False, subscribe=False, twist_cutoff=None, twist_outlier_thresh=None, ): """ Constructor. Parameters ---------- frame : str or ReferenceFrame Reference frame for which to publish the transform. base : str or ReferenceFrame, optional Base reference wrt to which the transform is published. Defaults to the parent reference frame. publish_pose : bool, default False If True, also publish a PoseStamped message on the topic "/<frame>/pose". publish_twist : bool, default False If True, also publish a TwistStamped message with the linear and angular velocity of the frame wrt the base on the topic "/<frame>/twist". subscribe : bool or str, default False If True, subscribe to the "/tf" topic and publish transforms when messages are broadcast whose `child_frame_id` is the name of the base frame. If the frame is a moving reference frame, the transform whose timestamp is the closest to the broadcast timestamp is published. `subscribe` can also be a string, in which case this broadcaster will be listening for TF messages with this `child_frame_id`. """ import pandas as pd import rospy import tf2_ros from geometry_msgs.msg import PoseStamped, TwistStamped from tf.msg import tfMessage self.frame = _resolve_rf(frame) self.base = _resolve_rf(base or self.frame.parent) ( self.translation, self.rotation, self.timestamps, ) = self.frame.lookup_transform(self.base) if self.timestamps is None: self.broadcaster = tf2_ros.StaticTransformBroadcaster() else: self.timestamps = pd.Index(self.timestamps) self.broadcaster = tf2_ros.TransformBroadcaster() if publish_pose: self.pose_publisher = rospy.Publisher( f"/{self.frame.name}/pose", PoseStamped, queue_size=1, latch=True, ) else: self.pose_publisher = None if publish_twist: self.linear, self.angular = self.frame.lookup_twist( reference=base, represent_in=self.frame, cutoff=twist_cutoff, outlier_thresh=twist_outlier_thresh, ) self.twist_publisher = rospy.Publisher( f"/{self.frame.name}/twist", TwistStamped, queue_size=1, latch=True, ) else: self.twist_publisher = None if subscribe: self.subscriber = rospy.Subscriber("/tf", tfMessage, self.handle_incoming_msg) if isinstance(subscribe, str): self.subscribe_to_frame = subscribe else: self.subscribe_to_frame = self.base.name else: self.subscriber = None self.idx = 0 self.stopped = False self._thread = None
def __init__(self, globalFrame="map"): # create an interactive marker server on the topic namespace areaSelectionMarkerServer self.server = InteractiveMarkerServer("areaSelectionMarkerServer") # Create a menu handler to use menu options on an interactive marker self.menu_handler = MenuHandler() # Create a transform broadcaster, this will be used to create a TF between the global frame, and a frame for all the markers and stripes to ride on self.br = tf2_ros.StaticTransformBroadcaster() # Publish an array of poses, on a topic called "path_poses", these messages are destined for the robot to give goal positions self.posePublisher = rospy.Publisher("path_poses", geometry_msgs.msg.PoseArray, queue_size=1) # Set some variables, these could be made into dyanmic variables later self.pathGap = 0.3 # Set the spacing between stripes self.robotRadius = 0.3 # Set the safety inset distance for the stripes compared to the box they are in self.yawAngle = 0.0 # Yaw angle relative to stripes frame self.globalFrame = globalFrame.strip( '/') # Take arguement from user and remove any accidental "/" self.markerFrame = "markerFrame" # Name of the TF frame things will be attached to self.moveName = "selectionMarker" # Name of the main marker for moving coverage area around self.rotationName = "rotationMarker" # Name of the co-located rotation marker initialPose = Pose() # Blank pose structure initialPose.orientation = Quaternion( *tf_conversions.transformations.quaternion_from_euler(0, 0, 0)) self.tfHandler( initialPose, self.markerFrame ) # Create TF message between global and marker frame based on initial marker position # Initialise the various markers self.initMainMarker( initialPose, self.globalFrame, self.moveName ) # Main movement marker - translates marker TF frame self.addRotation( initialPose, self.globalFrame, self.rotationName) # Rotation marker - rotates marker TF frame # Generate corners of the boundary box, initially with corners (-1,-1) and (1,1) in marker TF frame firstCornerPose = Pose() firstCornerPose.position.x = 0.4 firstCornerPose.position.y = -0.6 self.firstCornerName = "firstCorner" # 1st = "bottom-right" in TF frame firstCorner = self.initCornerMarker( firstCornerPose, self.markerFrame, self.firstCornerName, startingCorner=True) # Initialise marker # STRIPE LENGTH CHANGE HERE: secondCornerPose = Pose() secondCornerPose.position.x = 2.4 secondCornerPose.position.y = 1.0 self.secondCornerName = "secondCorner" # 2nd = "top-left" in TF frame secondCorner = self.initCornerMarker( secondCornerPose, self.markerFrame, self.secondCornerName, startingCorner=False) # Initialise marker # All the interactive markers have been initialised, draw the boundary lines, and draw the robot path as line_strip self.drawLines(self.updateAreaBoundary(), self.markerFrame) # Draw boundary box self.drawPath(self.updateAreaBoundary(), self.markerFrame) # Draw robot path #Setup menu entries and add the menu options to the main interactive marker self.coverageOption = self.menu_handler.insert( "Confirm Coverage", callback=self.beginCoverage) self.cancelOption = self.menu_handler.insert("Cancel", callback=self.cancelCb) self.menu_handler.apply(self.server, "selectionMarker") # Create blank pose array to contain goal positions later, with goals in the global frame self.poseArray = geometry_msgs.msg.PoseArray() self.poseArray.header.frame_id = self.globalFrame self.tfHandler( initialPose, self.markerFrame ) # Create TF message between global and marker frame based on initial marker position self.startIndicator() dynrecon = Server(CoverageSelectionConfig, self.dynReconfigCallback)
def __init__(self): # get parameters self.WHEEL_RADIUS = rospy.get_param('wheel_radius', 0.075) self.WHEEL_BASE = rospy.get_param('wheel_base', 0.4064) self.ENC_TPR = rospy.get_param('tpr', 1120.0) # initialize attributes self.state = [0, 0, 0] self.ang_quat = [0, 0, 0, 0] self.cur_vel = np.array([None, None]) # start main loop rospy.init_node('base_localizer', anonymous=True) # initialize ROS publishers and subscribers self.pose_pub = rospy.Publisher('base/pose', Pose, queue_size=10) rospy.Subscriber("left_motor/fb/vel", Float32, self.left_motor_vel_callback) rospy.Subscriber("right_motor/fb/vel", Float32, self.right_motor_vel_callback) while None in self.cur_vel: continue timer = time.time() rate = rospy.Rate(50) while not rospy.is_shutdown(): # perform runge-kutta update dt = time.time() - timer self.runge_kutta_update(dt) timer = time.time() # publish pose pose_msg = Pose() pose_msg.position.x = self.state[0] pose_msg.position.y = self.state[1] pose_msg.position.z = 0 pose_msg.orientation.x = self.ang_quat[0] pose_msg.orientation.y = self.ang_quat[1] pose_msg.orientation.z = self.ang_quat[2] pose_msg.orientation.w = self.ang_quat[3] self.pose_pub.publish(pose_msg) # send transform message to ROS broadcaster = tf2_ros.StaticTransformBroadcaster() tf_msg = TransformStamped() tf_msg.header.stamp = rospy.Time.now() tf_msg.header.frame_id = "odom" tf_msg.child_frame_id = "base_link" tf_msg.transform.translation.x = self.state[0] tf_msg.transform.translation.y = self.state[1] tf_msg.transform.translation.z = 0 tf_msg.transform.rotation.x = self.ang_quat[0] tf_msg.transform.rotation.y = self.ang_quat[1] tf_msg.transform.rotation.z = self.ang_quat[2] tf_msg.transform.rotation.w = self.ang_quat[3] broadcaster.sendTransform(tf_msg) rate.sleep()
def __init__(self): rospy.init_node('performance_tracker', anonymous=True) # check arguments self.run_name = rospy.get_param("~run_name", "run") run_duration = rospy.get_param("~run_duration", 107) self.sampling_time = rospy.get_param( "~sampling_time", 0.04) # The loop sleeps for this long min_buffer_size = run_duration / self.sampling_time # Minimum columsn for arrays containing periodically fetched data self.start_time = 0 self.groundtruth_frame = rospy.get_param("~groundtruth_frame", "fox") self.static_frame = rospy.get_param("~static_frame", "map") self.estimate_frame = rospy.get_param("~estimate_frame", "imu") self.groundtruth_topic = rospy.get_param("~groundtruth_topic", "") self.broadcast_groundtruth = rospy.get_param("~broadcast_groundtruth", False) self.trigger_topic = rospy.get_param("~trigger_topic", "/rovio/odometry") #tf setup self.tf_Buffer = tf.Buffer(rospy.Duration(10)) self.tf_listener = tf.TransformListener(self.tf_Buffer) self.tf_broadcaster = tf.TransformBroadcaster() self.tf_static_broadcaster = tf.StaticTransformBroadcaster() rospy.on_shutdown(self.save_data_to_disk) self.stampedTrajEstimate = PlotData(min_buffer_size, self.sampling_time) self.stampedGroundtruth = PlotData(min_buffer_size, self.sampling_time) self.anchors = [] if self.broadcast_groundtruth: rospy.loginfo(self.groundtruth_topic) rospy.Subscriber(self.groundtruth_topic, TransformStamped, self.groundtruth_received, queue_size=1) rospy.Subscriber(self.trigger_topic, Odometry, self.trigger_est, queue_size=1) rospy.Subscriber(self.trigger_topic, Odometry, self.trigger_gro, queue_size=1) rospy.Subscriber("asa_ros/found_anchor", FoundAnchor, self.found_anchor, queue_size=2) rospy.Subscriber("asa_ros_0/found_anchor", FoundAnchor, self.found_anchor, queue_size=2) rospy.Subscriber("asa_ros_1/found_anchor", FoundAnchor, self.found_anchor, queue_size=2) rospy.Subscriber("asa_ros_2/found_anchor", FoundAnchor, self.found_anchor, queue_size=2) rospy.Subscriber("asa_ros_3/found_anchor", FoundAnchor, self.found_anchor, queue_size=2) self.count = 0 rospy.loginfo("Tracking as groundtruth: " + self.groundtruth_frame) rospy.loginfo("Tracking as estimate: " + self.estimate_frame) rospy.loginfo("Using : " + self.static_frame + " as a base") self.spin()
def run(self): br = tf2_ros.StaticTransformBroadcaster() for i in self.tfMessages: i.header.stamp = rospy.Time.now() br.sendTransform(self.tfMessages)
def Broadcast(self): broadcaster = tf2_ros.StaticTransformBroadcaster() broadcaster.sendTransform(self.transforms_list) time.sleep(0.05)
'z': {'value': z1, 'step': 0.1}, 'azimuth': {'value': yaw1, 'step': 1}, 'pitch': {'value': pitch1, 'step': 1}, 'roll': {'value': roll1, 'step': 1}, 'message': ''} status2 = {'mode': 'pitch', 'x': {'value': x2, 'step': 0.1}, 'y': {'value': y2, 'step': 0.1}, 'z': {'value': z2, 'step': 0.1}, 'azimuth': {'value': yaw2, 'step': 1}, 'pitch': {'value': pitch2, 'step': 1}, 'roll': {'value': roll2, 'step': 1}, 'message': ''} rospy.init_node('my_static_tf2_broadcaster') broadcaster1 = tf2_ros.StaticTransformBroadcaster() broadcaster2 = tf2_ros.StaticTransformBroadcaster() status1_keys = [key[0] for key in status1.keys()] publish_status(broadcaster1, status1, 'cam_M_link', 'cam_R_link') time.sleep(1) status2_keys = [key[0] for key in status2.keys()] publish_status(broadcaster2, status2, 'cam_M_link', 'cam_L_link') print print 'Transforms published' print 'Press Q to quit' print
def __init__(self): rospy.init_node('drift_compensator', anonymous=True) # check arguments self.rovio_ns = rospy.get_param("~rovio_ns", "") self.anchor_id_preset = rospy.get_param("~anchor_id_preset", "") self.compensate_position = rospy.get_param("~compensate_position", True) self.compensate_rotation = rospy.get_param("~compensate_rotation", False) self.covariance_mode = rospy.get_param("~covariance_mode", "empirical") self.manage_asa_node = rospy.get_param("~manage_asa_node", False) self.use_alternative_facts = rospy.get_param("~use_alternative_facts", False) self.use_interpolation = rospy.get_param("~interpolate_alt", False) self.send_update_mode = rospy.get_param("~send_update", 0) #tf setup self.tf_Buffer = tf.Buffer(rospy.Duration(10)) self.tf_listener = tf.TransformListener(self.tf_Buffer) self.tf_broadcaster = tf.TransformBroadcaster() self.tf_static_broadcaster = tf.StaticTransformBroadcaster() # All publishers self.odometry_publisher = rospy.Publisher(self.rovio_ns + '/odometry', Odometry, queue_size=1) self.alternativeOdomPublisher = rospy.Publisher(self.rovio_ns + '/odometry_alt', Odometry, queue_size=1) self.info_publisher = rospy.Publisher("drift/debug", String, queue_size=1) if self.send_update_mode == 1: self.world_drift_observer = WorldDriftObserver( 0.5, self.rovio_drifted) # All subscribers rospy.Subscriber('/rovio/odometry', Odometry, self.rovio_odometry_received, queue_size=1) self.world_frame = "world" rospy.loginfo("Initialized anchor_manager") self.stateUpdater = {} self.use_multiple_anchors = rospy.get_param("~multiple_anchors", False) self.mutliple_anchor_mode = rospy.get_param( "~multiple_anchors_mode", 0) # 0: use the best, 1 use all self.did_update_with_anchor = {} self.block_updates_until_anchor = {} self.asa_handler = [] asa_nodes_to_launch = 2 if self.use_multiple_anchors else 1 self.asa_handler.append( asa_handler(self.manage_asa_node, self.anchor_id_preset, self.notify_anchor_found, self.report_last_anchor_id, 0)) for i in range(1, asa_nodes_to_launch): self.asa_handler.append( asa_handler(self.manage_asa_node, "", self.notify_anchor_found, self.report_last_anchor_id, i)) if self.anchor_id_preset != "": self.asa_handler[0].SetIsHoloLensAnchor(True)
def __init__(self): self.server = rospy.Service('broadcast_object_frames', BroadcastObjectFrames, self.__handle_broadcast_request) self.broadcaster = tf2_ros.StaticTransformBroadcaster() print("Initialized")
import rospy import tf2_ros from geometry_msgs.msg import TransformStamped # topics topic_local_pos = "/mavros/local_position/pose" # tf link topic_tf_link = "base_link" topic_stabilized_link = "base_stabilized" topic_tf_footprint = "base_footprint" # init params tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) tf_footprint_pose = tf2_ros.StaticTransformBroadcaster() tf_stabilized_pose = tf2_ros.StaticTransformBroadcaster() # main function if __name__ == '__main__': # init node rospy.init_node('msg_to_ft', anonymous=True) rate = rospy.Rate(100) # set rate tf_fp = TransformStamped() tf_fp.transform.rotation.w = 1.0 ft_stab = TransformStamped() while not rospy.is_shutdown():
def __init__(self): self.pub = rospy.Publisher('/usbl/pose_projected', Odometry, queue_size=10) self.pressed = False rospy.Subscriber("/usbl/latitude_longitude", NavSatFix, self.callback) # self.br = tf.TransformBroadcaster() self.br = tf2_ros.StaticTransformBroadcaster()
def publish_odom_transform(self): self.broadcaster = tf2_ros.StaticTransformBroadcaster() static_transformStamped = self.get_cur_ros_transform() self.broadcaster.sendTransform(static_transformStamped)
def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) ## parameters self.map_frame = rospy.get_param("~map_frame", 'odom') self.bot_frame = 'base_link' self.switch_thred = 1.5 # change to next lane if reach next self.pursuit = PurePursuit() self.lka = rospy.get_param("~lookahead", 0.5) self.pursuit.set_look_ahead_distance(self.lka) ## node path while not rospy.has_param("/guid_path") and not rospy.is_shutdown(): rospy.loginfo("Wait for /guid_path") rospy.sleep(0.5) self.guid_path = rospy.get_param("/guid_path") self.tag_ang_init = rospy.get_param("/guid_path_ang_init") self.last_node = -1 self.next_node_id = None self.all_anchor_ids = rospy.get_param("/all_anchor_ids") self.joy_lock = False self.get_goal = True self.joint_ang = None ## set first tracking lane self.pub_robot_speech = rospy.Publisher("speech_case", String, queue_size=1) self.pub_robot_go = rospy.Publisher("robot_go", Bool, queue_size=1) rospy.sleep(1) # wait for the publisher to be set well self.set_lane(True) # variable self.target_global = None self.listener = tf.TransformListener() # markers self.pub_goal_marker = rospy.Publisher("goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher("target_marker", Marker, queue_size=1) # publisher, subscriber self.pub_pid_goal = rospy.Publisher("pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber("anchor_position", PoseDirectional, self.cb_goal, queue_size=1) sub_joy = rospy.Subscriber('joy_teleop/joy', Joy, self.cb_joy, queue_size=1) sub_fr = rospy.Subscriber('front_right/ranges', DeviceRangeArray, self.cb_range, queue_size=1) sub_joint = rospy.Subscriber('/dynamixel_workbench/joint_states', JointState, self.cb_joint, queue_size=1) #Don't update goal too often self.last_update_goal = None self.goal_update_thred = 0.001 self.hist_goal = np.array([]) self.normal = True self.get_goal = True self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking) # print("init done") # prevent sudden stop self.last_plan = None # keep searching until find path or reach search end self.search_angle = 10 * math.pi / 180 self.search_max = 5 ### stupid range drawing # for using tf to draw range br1 = tf2_ros.StaticTransformBroadcaster() br2 = tf2_ros.StaticTransformBroadcaster() # stf.header.frame_id = "uwb_back_left" # stf.child_frame_id = "base_link" # stf.transform.translation.x = -1*r_tag_points[0, 0] # stf.transform.translation.y = -1*r_tag_points[0, 1] # br1.sendTransform(stf) # stf2 = stf # stf2.header.stamp = rospy.Time.now() # stf2.header.frame_id = "uwb_front_right" # stf2.transform.translation.x = -1*r_tag_points[1, 0] # stf2.transform.translation.y = -1*r_tag_points[1, 1] # br2.sendTransform(stf2) stf = TransformStamped() stf.header.stamp = rospy.Time.now() stf.transform.rotation.w = 1 stf.header.frame_id = "base_link" stf.child_frame_id = "uwb_back_left" stf.transform.translation.x = r_tag_points[0, 0] stf.transform.translation.y = r_tag_points[0, 1] stf2 = TransformStamped() stf2.header.stamp = rospy.Time.now() stf2.transform.rotation.w = 1 stf2.header.frame_id = "base_link" stf2.child_frame_id = "uwb_front_right" stf2.transform.translation.x = r_tag_points[1, 0] stf2.transform.translation.y = r_tag_points[1, 1]
def __init__(self, real_robot=False, ur_model= 'ur10'): # Event is clear while initialization or set_state is going on self.reset = Event() self.reset.clear() self.get_state_event = Event() self.get_state_event.set() self.real_robot = real_robot self.ur_model = ur_model # Joint States self.joint_names = ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', \ 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] self.joint_position = dict.fromkeys(self.joint_names, 0.0) self.joint_velocity = dict.fromkeys(self.joint_names, 0.0) rospy.Subscriber("joint_states", JointState, self._on_joint_states) # Robot control self.arm_cmd_pub = rospy.Publisher('env_arm_command', JointTrajectory, queue_size=1) # joint_trajectory_command_handler publisher self.sleep_time = (1.0 / rospy.get_param("~action_cycle_rate")) - 0.01 self.control_period = rospy.Duration.from_sec(self.sleep_time) self.max_velocity_scale_factor = float(rospy.get_param("~max_velocity_scale_factor")) self.min_traj_duration = 0.5 # minimum trajectory duration (s) self.joint_velocity_limits = self._get_joint_velocity_limits() # Robot frames self.reference_frame = rospy.get_param("~reference_frame", "base") self.ee_frame = 'tool0' # TF2 self.tf2_buffer = tf2_ros.Buffer() self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer) self.static_tf2_broadcaster = tf2_ros.StaticTransformBroadcaster() # Collision detection if not self.real_robot: rospy.Subscriber("shoulder_collision", ContactsState, self._on_shoulder_collision) rospy.Subscriber("upper_arm_collision", ContactsState, self._on_upper_arm_collision) rospy.Subscriber("forearm_collision", ContactsState, self._on_forearm_collision) rospy.Subscriber("wrist_1_collision", ContactsState, self._on_wrist_1_collision) rospy.Subscriber("wrist_2_collision", ContactsState, self._on_wrist_2_collision) rospy.Subscriber("wrist_3_collision", ContactsState, self._on_wrist_3_collision) # Initialization of collision sensor flags self.collision_sensors = dict.fromkeys(["shoulder", "upper_arm", "forearm", "wrist_1", "wrist_2", "wrist_3"], False) # Robot Server mode rs_mode = rospy.get_param('~rs_mode') if rs_mode: self.rs_mode = rs_mode else: self.rs_mode = rospy.get_param("~target_mode", '1object') # Objects Controller self.objects_controller = rospy.get_param("objects_controller", False) self.n_objects = int(rospy.get_param("n_objects")) if self.objects_controller: self.move_objects_pub = rospy.Publisher('move_objects', Bool, queue_size=10) # Get objects model name self.objects_model_name = [] for i in range(self.n_objects): self.objects_model_name.append(rospy.get_param("object_" + repr(i) + "_model_name")) # Get objects TF Frame self.objects_frame = [] for i in range(self.n_objects): self.objects_frame.append(rospy.get_param("object_" + repr(i) + "_frame")) # Voxel Occupancy self.use_voxel_occupancy = rospy.get_param("~use_voxel_occupancy", False) if self.use_voxel_occupancy: rospy.Subscriber("occupancy_state", Int32MultiArray, self._on_occupancy_state) if self.rs_mode == '1moving1point_2_2_4_voxel': self.voxel_occupancy = [0.0] * 16
def __init__(self, pybullet, robot, **kargs): # get "import pybullet as pb" and store in self.pb self.pb = pybullet # get robot from parent class self.robot = robot # create image msg placeholder for publication self.image_msg = Image() # get RGBD camera parameters from ROS param server self.image_msg.width = rospy.get_param('~rgbd_camera/resolution/width', 640) self.image_msg.height = rospy.get_param( '~rgbd_camera/resolution/height', 480) assert (self.image_msg.width > 5) assert (self.image_msg.height > 5) cam_frame_id = rospy.get_param('~rgbd_camera/frame_id', None) if not cam_frame_id: rospy.logerr( 'Required parameter rgbd_camera/frame_id not set, will exit now...' ) rospy.signal_shutdown( 'Required parameter rgbd_camera/frame_id not set') return # get pybullet camera link id from its name link_names_to_ids_dic = kargs['link_ids'] if not cam_frame_id in link_names_to_ids_dic: rospy.logerr( 'Camera reference frame "{}" not found in URDF model'.format( cam_frame_id)) rospy.logwarn( 'Available frames are: {}'.format(link_names_to_ids_dic)) rospy.signal_shutdown( 'required param rgbd_camera/frame_id not set properly') return self.pb_camera_link_id = link_names_to_ids_dic[cam_frame_id] self.image_msg.header.frame_id = cam_frame_id # create publisher self.pub_image = rospy.Publisher('rgb_image', Image, queue_size=1) self.image_msg.encoding = rospy.get_param( '~rgbd_camera/resolution/encoding', 'rgb8') self.image_msg.is_bigendian = rospy.get_param( '~rgbd_camera/resolution/encoding', 0) self.image_msg.step = rospy.get_param( '~rgbd_camera/resolution/encoding', 1920) # projection matrix self.hfov = rospy.get_param('~rgbd_camera/hfov', 56.3) self.vfov = rospy.get_param('~rgbd_camera/vfov', 43.7) self.near_plane = rospy.get_param('~rgbd_camera/near_plane', 0.4) self.far_plane = rospy.get_param('~rgbd_camera/far_plane', 8) self.projection_matrix = self.compute_projection_matrix() # use cv_bridge ros to convert cv matrix to ros format self.image_bridge = CvBridge() # variable used to run this plugin at a lower frequency, HACK self.count = 0 ## Setting TF static transform from camera to point cloud data in required direction point_cloud_frame = "point_cloud_camera" broadcaster = tf2_ros.StaticTransformBroadcaster() static_transformStamped = geometry_msgs.msg.TransformStamped() static_transformStamped.header.stamp = rospy.Time.now() static_transformStamped.header.frame_id = cam_frame_id static_transformStamped.child_frame_id = point_cloud_frame static_transformStamped.transform.translation.x = 0 static_transformStamped.transform.translation.y = 0 static_transformStamped.transform.translation.z = 0 quat = tf.transformations.quaternion_from_euler(np.deg2rad(-90), 0, 0) static_transformStamped.transform.rotation.x = quat[0] static_transformStamped.transform.rotation.y = quat[1] static_transformStamped.transform.rotation.z = quat[2] static_transformStamped.transform.rotation.w = quat[3] broadcaster.sendTransform(static_transformStamped) # publisher for depth image self.pub_depth_image = rospy.Publisher('depth_image', Image, queue_size=1) # image msg for depth image self.depth_image_msg = Image() self.depth_image_msg.width = rospy.get_param( '~rgbd_camera/resolution/width', 640) self.depth_image_msg.height = rospy.get_param( '~rgbd_camera/resolution/height', 480) self.depth_image_msg.encoding = rospy.get_param( '~rgbd_camera/resolution/encoding', '32FC1') self.depth_image_msg.is_bigendian = rospy.get_param( '~rgbd_camera/resolution/encoding', 0) self.depth_image_msg.step = rospy.get_param( '~rgbd_camera/resolution/encoding', 2560) # publisher for point_cloud self.pub_point_cloud = rospy.Publisher('point_cloud', PointCloud2, queue_size=1) # point cloud msg self.point_cloud_msg = PointCloud2() self.point_cloud_msg.header.frame_id = point_cloud_frame ## The new frame is the static frame self.point_cloud_msg.width = rospy.get_param( '~rgbd_camera/resolution/width', 640) self.point_cloud_msg.height = rospy.get_param( '~rgbd_camera/resolution/height', 480) self.point_cloud_msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] self.point_cloud_msg.is_bigendian = rospy.get_param( '~rgbd_camera/resolution/encoding', 0) self.point_cloud_msg.point_step = 12 self.point_cloud_msg.row_step = self.point_cloud_msg.width * self.point_cloud_msg.point_step self.point_cloud_msg.is_dense = True # organised point cloud
def __init__(self): self.tfBcaster = tf2_ros.TransformBroadcaster() # FIXME, use that for ENU to NED self.static_tfBcaster = tf2_ros.StaticTransformBroadcaster() self.tfBuffer = tf2_ros.Buffer() self.tfLstener = tf2_ros.TransformListener(self.tfBuffer)
def __init__(self, arg): # Use sim clock rospy.set_param('/use_sim_time', 'true') # TF Initializations self.listener = tf.TransformListener() self.br = tf2_ros.StaticTransformBroadcaster() # Set Default variables self.delay_after_start_command = 2.0 self.sim_health = SimHealth() self.ecu = Ecu() # ROS Subscribers self.sub_topics_health = rospy.Subscriber('/fssim/topics_health', TopicsHealth, self.callback_topics_health) self.sub_state = rospy.Subscriber('/fssim/base_pose_ground_truth', State, self.callback_state) self.sub_mission_finished = rospy.Subscriber('/fssim/mission_finished', Mission, self.callback_mission_finished) # ROS Publishers self.pub_res = rospy.Publisher('/fssim/res_state', ResState, queue_size = 1) self.pub_health = rospy.Publisher('/fssim/health', SimHealth, queue_size = 1) self.pub_mission = rospy.Publisher('/fssim/mission', Mission, queue_size = 1, latch = True) self.pub_initialpose = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size = 1) with open(arg.config, 'r') as f: self.sim_config = yaml.load(f) self.sim_config_id = arg.sim_id self.checks_is_in_track = self.sim_config["res"]["checks"]["is_in_track"] self.robot_name = self.sim_config["robot_name"] rospy.set_param("robot_name", self.robot_name) # Initialize all needed Commands self.cmd_autonomous_system = None self.cmd_fssim = None self.cmd_rosbag = None self.mission = Mission() if "skidpad" in self.sim_config['repetitions'][self.sim_config_id]['track_name']: self.mission.mission = "skidpad" self.checks_is_in_track = False elif "acceleration" in self.sim_config['repetitions'][self.sim_config_id]['track_name']: self.mission.mission = "acceleration" self.checks_is_in_track = False else: self.mission.mission = "trackdrive" rospy.logwarn("Mission: %s", self.mission.mission) self.pub_mission.publish(self.mission) self.output_folder = arg.output self.statistics = LapStaticstic(self.output_folder, self.mission.mission) self.track_checks = VehiclePositionCheck(self.mission.mission, self.checks_is_in_track) signal.signal(signal.SIGINT, self.signal_handler) # Initialize starting time self.start_time = 0
def __init__(self, node_name): super(FusedLocalizationNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh_name = rospy.get_namespace().strip("/") self.radius = rospy.get_param( f'/{self.veh_name}/kinematics_node/radius', 100) self.baseline = 0.0968 x_init = rospy.get_param(f'/{self.veh_name}/{node_name}/x_init', 100) self.rectify_flag = rospy.get_param( f'/{self.veh_name}/{node_name}/rectify', 100) self.encoder_conf_flag = False self.bridge = CvBridge() self.image = None self.image_timestamp = rospy.Time.now() self.cam_info = None self.camera_info_received = False self.newCameraMatrix = None self.at_detector = Detector(families='tag36h11', nthreads=1, quad_decimate=1.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) trans_base_cam = np.array([0.0582, 0.0, 0.1072]) rot_base_cam = rotation_from_axis_angle(np.array([0, 1, 0]), np.radians(15)) rot_cam_base = rot_base_cam.T trans_cam_base = -rot_cam_base @ trans_base_cam self.pose_cam_base = SE3_from_rotation_translation( rot_cam_base, trans_cam_base) self.tfs_msg_cam_base = TransformStamped() self.tfs_msg_cam_base.header.frame_id = 'camera' self.tfs_msg_cam_base.header.stamp = rospy.Time.now() self.tfs_msg_cam_base.child_frame_id = 'at_baselink' self.tfs_msg_cam_base.transform = self.pose2transform( self.pose_cam_base) self.static_tf_br_cam_base = tf2_ros.StaticTransformBroadcaster() translation_map_at = np.array([0.0, 0.0, 0.08]) rot_map_at = np.eye(3) self.pose_map_at = SE3_from_rotation_translation( rot_map_at, translation_map_at) self.tfs_msg_map_april = TransformStamped() self.tfs_msg_map_april.header.frame_id = 'map' self.tfs_msg_map_april.header.stamp = rospy.Time.now() self.tfs_msg_map_april.child_frame_id = 'apriltag' self.tfs_msg_map_april.transform = self.pose2transform( self.pose_map_at) self.static_tf_br_map_april = tf2_ros.StaticTransformBroadcaster() self.tfs_msg_april_cam = TransformStamped() self.tfs_msg_april_cam.header.frame_id = 'apriltag' self.tfs_msg_april_cam.header.stamp = rospy.Time.now() self.tfs_msg_april_cam.child_frame_id = 'camera' self.br_april_cam = tf.TransformBroadcaster() self.tfs_msg_map_base = TransformStamped() self.tfs_msg_map_base.header.frame_id = 'map' self.tfs_msg_map_base.header.stamp = rospy.Time.now() self.tfs_msg_map_base.child_frame_id = 'fused_baselink' self.br_map_base = tf.TransformBroadcaster() self.pose_map_base_SE2 = SE2_from_xytheta([x_init, 0, np.pi]) R_x_c = rotation_from_axis_angle(np.array([1, 0, 0]), np.pi / 2) R_z_c = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2) R_y_a = rotation_from_axis_angle(np.array([0, 1, 0]), -np.pi / 2) R_z_a = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2) R_dtc_c = R_x_c @ R_z_c R_a_dta = R_y_a @ R_z_a self.T_dtc_c = SE3_from_rotation_translation(R_dtc_c, np.array([0, 0, 0])) self.T_a_dta = SE3_from_rotation_translation(R_a_dta, np.array([0, 0, 0])) self.sub_image_comp = rospy.Subscriber( f'/{self.veh_name}/camera_node/image/compressed', CompressedImage, self.image_cb, buff_size=10000000, queue_size=1) self.sub_cam_info = rospy.Subscriber( f'/{self.veh_name}/camera_node/camera_info', CameraInfo, self.cb_camera_info, queue_size=1) self.pub_tf_fused_loc = rospy.Publisher( f'/{self.veh_name}/{node_name}/transform_stamped', TransformStamped, queue_size=10)
def __init__(self, ref_frame='map', robot_name='kawada', scene_object_factory=None): # self.scene_objects = [] rospy.init_node("SceneGraphMockupManager") # TODO: make the roots configurable # we save object names here, that should not be deleted self.keep_frames = set() # assert isinstance(scene_object_factory, SceneObjectFactory) self.scene_object_factory = scene_object_factory # type: SceneObjectFactory self.robot_info = RobotInfo.getRobotInfo() # type: RobotInfo self.root = ['map'] self.scene_objects_dict = {} self.static_transforms = [] # self.static_transforms += self.robot_info.getStaticRobotTransforms(rospy.Time.now()) self.static_transforms += self.scene_object_factory.getStaticTransforms( rospy.Time.now()) self.add_transforms(self.static_transforms) # self.add_transforms(self.scene_object_factory.getStaticTransforms(rospy.Time.now())) topic = 'visualization_marker_array' self.marker_publisher = rospy.Publisher( topic, visualization_msgs.msg.MarkerArray, queue_size=100) self.br = tf2_ros.TransformBroadcaster() self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) self.scene = moveit_commander.PlanningSceneInterface() self.static_transform_broadcaster = tf2_ros.StaticTransformBroadcaster( ) self.timer = rospy.Timer(rospy.Duration(0.5), self.handle_publish_transforms_timer) rospy.on_shutdown(self.shutdown_hook) rospy.sleep(1.0) # TODO: factor static transforms out # self.gripper_static_transform(robot_name=robot_name) # self.init_robot_transform(robot_name=robot_name) self.init_tree_roots(ref_frame=ref_frame, robot_name=robot_name) # Open service for object query by type self.object_name_by_type = rospy.Service( 'SceneGraphMockupManager/getObjectNamesByType', StringQuery, self.handle_get_object_names_by_type) self.object_types_in_scene = rospy.Service( 'SceneGraphMockupManager/getObjectTypesInScene', StringQuery, self.handle_get_object_types) self.object_pose_by_name = rospy.Service( 'SceneGraphMockupManager/getObjectPoseByName', StringQuery, self.handle_get_object_poses_by_names) self.object_del_by_name = rospy.Service( 'SceneGraphMockupManager/delObjectsByName', StringQuery, self.handle_del_objects_by_name) self.objects_by_type = rospy.Service( 'SceneGraphMockupManager/getObjectsByType', StringQuery, self.handle_get_objects_by_type) self.set_object_color_by_name = rospy.Service( 'SceneGraphMockupManager/setObjectColorByName', StringQuery, self.handle_set_object_color_by_name) self.reload_scene = rospy.Service( 'SceneGraphMockupManager/reloadSceneById', StringQuery, self.handle_reload_scene_by_id) self.add_object_srv = rospy.Service( 'SceneGraphMockupManager/addObject', AddObject, self.handle_add_object) self.get_object_types_by_name = rospy.Service( 'SceneGraphMockupManager/getObjectTypesByName', StringQuery, self.handle_get_object_types_by_name) rospy.spin()
def broadcast_tf(self, x, y, th): tf2_ros.StaticTransformBroadcaster().sendTransform( (x, y, 0.0), tf.transformations.quaternion_from_euler(0, 0, th), rospy.Time.now(), "/food_marker", "/map")
def main(self): """Main function for the SpotROS class. Gets config from ROS and initializes the wrapper. Holds lease from wrapper and updates all async tasks at the ROS rate""" rospy.init_node('spot_ros', anonymous=True) rate = rospy.Rate(50) self.rates = rospy.get_param('~rates', {}) self.username = rospy.get_param('~username', 'default_value') self.password = rospy.get_param('~password', 'default_value') self.hostname = rospy.get_param('~hostname', 'default_value') self.motion_deadzone = rospy.get_param('~deadzone', 0.05) self.estop_timeout = rospy.get_param('~estop_timeout', 9.0) self.camera_static_transform_broadcaster = tf2_ros.StaticTransformBroadcaster() # Static transform broadcaster is super simple and just a latched publisher. Every time we add a new static # transform we must republish all static transforms from this source, otherwise the tree will be incomplete. # We keep a list of all the static transforms we already have so they can be republished, and so we can check # which ones we already have self.camera_static_transforms = [] # Spot has 2 types of odometries: 'odom' and 'vision' # The former one is kinematic odometry and the second one is a combined odometry of vision and kinematics # These params enables to change which odometry frame is a parent of body frame and to change tf names of each odometry frames. self.mode_parent_odom_tf = rospy.get_param('~mode_parent_odom_tf', 'odom') # 'vision' or 'odom' self.tf_name_kinematic_odom = rospy.get_param('~tf_name_kinematic_odom', 'odom') self.tf_name_raw_kinematic = 'odom' self.tf_name_vision_odom = rospy.get_param('~tf_name_vision_odom', 'vision') self.tf_name_raw_vision = 'vision' if self.mode_parent_odom_tf != self.tf_name_raw_kinematic and self.mode_parent_odom_tf != self.tf_name_raw_vision: rospy.logerr('rosparam \'~mode_parent_odom_tf\' should be \'odom\' or \'vision\'.') return self.logger = logging.getLogger('rosout') rospy.loginfo("Starting ROS driver for Spot") self.spot_wrapper = SpotWrapper(self.username, self.password, self.hostname, self.logger, self.estop_timeout, self.rates, self.callbacks) if self.spot_wrapper.is_valid: # Images # self.back_image_pub = rospy.Publisher('camera/back/image', Image, queue_size=10) self.frontleft_image_pub = rospy.Publisher('camera/frontleft/image', Image, queue_size=10) self.frontright_image_pub = rospy.Publisher('camera/frontright/image', Image, queue_size=10) self.left_image_pub = rospy.Publisher('camera/left/image', Image, queue_size=10) self.right_image_pub = rospy.Publisher('camera/right/image', Image, queue_size=10) # Depth # self.back_depth_pub = rospy.Publisher('depth/back/image', Image, queue_size=10) self.frontleft_depth_pub = rospy.Publisher('depth/frontleft/image', Image, queue_size=10) self.frontright_depth_pub = rospy.Publisher('depth/frontright/image', Image, queue_size=10) self.left_depth_pub = rospy.Publisher('depth/left/image', Image, queue_size=10) self.right_depth_pub = rospy.Publisher('depth/right/image', Image, queue_size=10) # Image Camera Info # self.back_image_info_pub = rospy.Publisher('camera/back/camera_info', CameraInfo, queue_size=10) self.frontleft_image_info_pub = rospy.Publisher('camera/frontleft/camera_info', CameraInfo, queue_size=10) self.frontright_image_info_pub = rospy.Publisher('camera/frontright/camera_info', CameraInfo, queue_size=10) self.left_image_info_pub = rospy.Publisher('camera/left/camera_info', CameraInfo, queue_size=10) self.right_image_info_pub = rospy.Publisher('camera/right/camera_info', CameraInfo, queue_size=10) # Depth Camera Info # self.back_depth_info_pub = rospy.Publisher('depth/back/camera_info', CameraInfo, queue_size=10) self.frontleft_depth_info_pub = rospy.Publisher('depth/frontleft/camera_info', CameraInfo, queue_size=10) self.frontright_depth_info_pub = rospy.Publisher('depth/frontright/camera_info', CameraInfo, queue_size=10) self.left_depth_info_pub = rospy.Publisher('depth/left/camera_info', CameraInfo, queue_size=10) self.right_depth_info_pub = rospy.Publisher('depth/right/camera_info', CameraInfo, queue_size=10) # Status Publishers # self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10) """Defining a TF publisher manually because of conflicts between Python3 and tf""" self.tf_pub = rospy.Publisher('tf', TFMessage, queue_size=10) self.metrics_pub = rospy.Publisher('status/metrics', Metrics, queue_size=10) self.lease_pub = rospy.Publisher('status/leases', LeaseArray, queue_size=10) self.odom_twist_pub = rospy.Publisher('odometry/twist', TwistWithCovarianceStamped, queue_size=10) self.odom_pub = rospy.Publisher('odometry', Odometry, queue_size=10) self.feet_pub = rospy.Publisher('status/feet', FootStateArray, queue_size=10) self.estop_pub = rospy.Publisher('status/estop', EStopStateArray, queue_size=10) self.wifi_pub = rospy.Publisher('status/wifi', WiFiState, queue_size=10) self.power_pub = rospy.Publisher('status/power_state', PowerState, queue_size=10) self.battery_pub = rospy.Publisher('status/battery_states', BatteryStateArray, queue_size=10) self.behavior_faults_pub = rospy.Publisher('status/behavior_faults', BehaviorFaultState, queue_size=10) self.system_faults_pub = rospy.Publisher('status/system_faults', SystemFaultState, queue_size=10) self.feedback_pub = rospy.Publisher('status/feedback', Feedback, queue_size=10) self.mobility_params_pub = rospy.Publisher('status/mobility_params', MobilityParams, queue_size=10) rospy.Subscriber('cmd_vel', Twist, self.cmdVelCallback, queue_size = 1) rospy.Subscriber('body_pose', Pose, self.bodyPoseCallback, queue_size = 1) rospy.Service("claim", Trigger, self.handle_claim) rospy.Service("release", Trigger, self.handle_release) rospy.Service("stop", Trigger, self.handle_stop) rospy.Service("self_right", Trigger, self.handle_self_right) rospy.Service("sit", Trigger, self.handle_sit) rospy.Service("stand", Trigger, self.handle_stand) rospy.Service("power_on", Trigger, self.handle_power_on) rospy.Service("power_off", Trigger, self.handle_safe_power_off) rospy.Service("estop/hard", Trigger, self.handle_estop_hard) rospy.Service("estop/gentle", Trigger, self.handle_estop_soft) rospy.Service("estop/release", Trigger, self.handle_estop_disengage) rospy.Service("stair_mode", SetBool, self.handle_stair_mode) rospy.Service("locomotion_mode", SetLocomotion, self.handle_locomotion_mode) rospy.Service("max_velocity", SetVelocity, self.handle_max_vel) rospy.Service("clear_behavior_fault", ClearBehaviorFault, self.handle_clear_behavior_fault) rospy.Service("list_graph", ListGraph, self.handle_list_graph) self.navigate_as = actionlib.SimpleActionServer('navigate_to', NavigateToAction, execute_cb = self.handle_navigate_to, auto_start = False) self.navigate_as.start() self.trajectory_server = actionlib.SimpleActionServer("trajectory", TrajectoryAction, execute_cb=self.handle_trajectory, auto_start=False) self.trajectory_server.start() rospy.on_shutdown(self.shutdown) self.auto_claim = rospy.get_param('~auto_claim', False) self.auto_power_on = rospy.get_param('~auto_power_on', False) self.auto_stand = rospy.get_param('~auto_stand', False) if self.auto_claim: self.spot_wrapper.claim() if self.auto_power_on: self.spot_wrapper.power_on() if self.auto_stand: self.spot_wrapper.stand() while not rospy.is_shutdown(): self.spot_wrapper.updateTasks() feedback_msg = Feedback() feedback_msg.standing = self.spot_wrapper.is_standing feedback_msg.sitting = self.spot_wrapper.is_sitting feedback_msg.moving = self.spot_wrapper.is_moving id = self.spot_wrapper.id try: feedback_msg.serial_number = id.serial_number feedback_msg.species = id.species feedback_msg.version = id.version feedback_msg.nickname = id.nickname feedback_msg.computer_serial_number = id.computer_serial_number except: pass self.feedback_pub.publish(feedback_msg) mobility_params_msg = MobilityParams() try: mobility_params = self.spot_wrapper.get_mobility_params() mobility_params_msg.body_control.position.x = \ mobility_params.body_control.base_offset_rt_footprint.points[0].pose.position.x mobility_params_msg.body_control.position.y = \ mobility_params.body_control.base_offset_rt_footprint.points[0].pose.position.y mobility_params_msg.body_control.position.z = \ mobility_params.body_control.base_offset_rt_footprint.points[0].pose.position.z mobility_params_msg.body_control.orientation.x = \ mobility_params.body_control.base_offset_rt_footprint.points[0].pose.rotation.x mobility_params_msg.body_control.orientation.y = \ mobility_params.body_control.base_offset_rt_footprint.points[0].pose.rotation.y mobility_params_msg.body_control.orientation.z = \ mobility_params.body_control.base_offset_rt_footprint.points[0].pose.rotation.z mobility_params_msg.body_control.orientation.w = \ mobility_params.body_control.base_offset_rt_footprint.points[0].pose.rotation.w mobility_params_msg.locomotion_hint = mobility_params.locomotion_hint mobility_params_msg.stair_hint = mobility_params.stair_hint except Exception as e: rospy.logerr('Error:{}'.format(e)) pass self.mobility_params_pub.publish(mobility_params_msg) rate.sleep()
def __init__(self): super(BeltInterpreter, self).__init__() self.SENSOR_FRAME_ID = "belt_{}" # {} will be replaced by the sensor name rospy.init_node("belt_interpreter") rospy.logdebug("Node started") # get definition file rospy.wait_for_service('/memory/definitions/get') get_def = rospy.ServiceProxy('/memory/definitions/get', GetDefinition) try: res = get_def("processing/belt.xml") if not res.success: rospy.logerr("Error when fetching belt definition file") def_filename = res.path except rospy.ServiceException as exc: rospy.logerr("Error when fetching belt definition file: {}".format( str(exc))) raise Exception() rospy.logdebug("Belt definition file fetched") # parse definition file self._belt_parser = BeltParser(def_filename) self._sensors_sub = rospy.Subscriber("/sensors/belt", RangeList, self.callback) self._pub = rospy.Publisher("/processing/belt_interpreter/points", BeltFiltered, queue_size=10) self._tl = tf.TransformListener() self._broadcaster = tf2_ros.StaticTransformBroadcaster() self._markers_pub = MarkersPublisher() self.pub_static_transforms() rospy.logdebug("Subscribed to sensors topics") self._static_shapes = [] rospy.wait_for_service('/memory/map/get') get_map = rospy.ServiceProxy('/memory/map/get', MapGet) response = get_map("/terrain/*") if not response.success: rospy.logerr("Error when fetching objects from map") else: map_obj = json.loads(get_map("/terrain/*").response) for v in map_obj["walls"]["layer_ground"].values(): x = float(v["position"]["x"]) y = float(v["position"]["y"]) type = v["shape"]["type"] if type == "rect": w = float(v["shape"]["width"]) h = float(v["shape"]["height"]) shape = Rectangle(x, y, w, h) elif type == "circle": r = float(v["shape"]["radius"]) shape = Circle(x, y, r) else: # TODO POLYGONS pass self._static_shapes.append(shape) rospy.spin()
rate = rospy.Rate(10.0) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) while not rospy.is_shutdown(): try: trans = tfBuffer.lookup_transform('base_link', 'landmark_location', rospy.Time()) print "landmark_location wrt base_link" print trans.transform break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() continue broadcaster2 = tf2_ros.StaticTransformBroadcaster() landmark_to_obj = geometry_msgs.msg.TransformStamped() landmark_to_obj.header.stamp = rospy.Time.now() landmark_to_obj.header.frame_id = "landmark_location" landmark_to_obj.child_frame_id = "object_location" #input offset here (found from moma_find_offset.py program) landmark_to_obj.transform.translation.x = 12.7157474008 landmark_to_obj.transform.translation.y = 220.10747054 landmark_to_obj.transform.translation.z = -211.602548852 # quat = tf.transformations.quaternion_from_euler( # 42.0302727731,6.08534532397,-122.612682455) quat = tf.transformations.quaternion_from_euler(0, 0, 0) landmark_to_obj.transform.rotation.x = quat[0] landmark_to_obj.transform.rotation.y = quat[1] landmark_to_obj.transform.rotation.z = quat[2] landmark_to_obj.transform.rotation.w = quat[3]
import sys import tf import tf2_ros import geometry_msgs.msg if __name__ == '__main__': # if len(sys.argv) < 8 : # rospy.logerr('Invalid number of parameters\nusage: ./static_turtle_tf2_broadcaster.py child_frame_name x y z roll pitch yaw') # sys.exit(0) # else: # if sys.argv[1] == 'world': # rospy.logerr('Your static turtle name cannot be "world"') # sys.exit(0) rospy.init_node('my_static_tf2_broadcaster') broadcaster = tf2_ros.StaticTransformBroadcaster() static_transformStamped = geometry_msgs.msg.TransformStamped() static_transformStamped.header.stamp = rospy.Time.now() static_transformStamped.header.frame_id = "/torso_lift_link" static_transformStamped.child_frame_id = "/test" static_transformStamped.transform.translation.x = 0 static_transformStamped.transform.translation.y = -1.0 static_transformStamped.transform.translation.z = 1.0 static_transformStamped.transform.rotation.x = 0 static_transformStamped.transform.rotation.y = 1 static_transformStamped.transform.rotation.z = 0 static_transformStamped.transform.rotation.w = 0
def __init__(self, node_name): # Initialize the DTROS parent class super(ATLocalizationNode, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.veh = rospy.get_namespace().strip("/") # State variable for the robot self.pose = Pose2D(0.27, 0.0, np.pi) # Initial state given arbitrarily # Static transforms # Map -> Baselink. To be computed self.T_map_baselink = TransformStamped() self.T_map_baselink.header.frame_id = 'map' self.T_map_baselink.child_frame_id = 'at_baselink' # Map -> Apriltag. STATIC self._T_map_apriltag = TransformStamped() self._T_map_apriltag.header.frame_id = 'map' self._T_map_apriltag.header.stamp = rospy.Time.now() self._T_map_apriltag.child_frame_id = 'apriltag' self._T_map_apriltag.transform.translation.x = 0.0 self._T_map_apriltag.transform.translation.y = 0.0 self._T_map_apriltag.transform.translation.z = 0.09 # Height of 9 cm q = tf_conversions.transformations.quaternion_from_euler(0.0, 0.0, 0.0) self._T_map_apriltag.transform.rotation.x = q[0] self._T_map_apriltag.transform.rotation.y = q[1] self._T_map_apriltag.transform.rotation.z = q[2] self._T_map_apriltag.transform.rotation.w = q[3] # Apriltag -> Camera. Computed using apriltag detection self.T_apriltag_camera = TransformStamped() self.T_apriltag_camera.header.frame_id = 'apriltag' self.T_apriltag_camera.child_frame_id = 'camera' # Camera -> Baselink. STATIC self._T_camera_baselink = TransformStamped() self._T_camera_baselink.header.frame_id = 'camera' self._T_camera_baselink.header.stamp = rospy.Time.now() self._T_camera_baselink.child_frame_id = 'at_baselink' self._T_camera_baselink.transform.translation.x = -0.0582 self._T_camera_baselink.transform.translation.y = 0.0 self._T_camera_baselink.transform.translation.z = -0.110 q = tf_conversions.transformations.quaternion_from_euler( 0.0, np.deg2rad(-15), 0.0) self._T_camera_baselink.transform.rotation.x = q[0] self._T_camera_baselink.transform.rotation.y = q[1] self._T_camera_baselink.transform.rotation.z = q[2] self._T_camera_baselink.transform.rotation.w = q[3] # Transformation Matrices to ease computation R_MA = tf_conversions.transformations.euler_matrix( 0.0, 0.0, 0.0, 'rxyz') t_MA = tf_conversions.transformations.translation_matrix( np.array([0.0, 0.0, 0.09])) self.T_MA = tf_conversions.transformations.concatenate_matrices( t_MA, R_MA) R_CB = tf_conversions.transformations.euler_matrix( 0.0, np.deg2rad(-15.0), 0.0, 'rxyz') t_CB = tf_conversions.transformations.translation_matrix( np.array([-0.0582, 0.0, -0.1072])) self.T_CB = tf_conversions.transformations.concatenate_matrices( t_CB, R_CB) # Define rotation matrices to follow axis convention in rviz when using apriltag detection self.T_ApA = tf_conversions.transformations.euler_matrix( np.pi / 2.0, 0.0, -np.pi / 2.0, 'rxyz') self.T_CCp = tf_conversions.transformations.euler_matrix( -np.pi / 2.0, 0.0, -np.pi / 2.0, 'rzyx') # Load calibration files self.calib_data = self.readYamlFile( '/data/config/calibrations/camera_intrinsic/' + self.veh + '.yaml') self.log('Loaded intrinsics calibration file') self.extrinsics = self.readYamlFile( '/data/config/calibrations/camera_extrinsic/' + self.veh + '.yaml') self.log('Loaded extrinsics calibration file') # Retrieve intrinsic info cam_info = self.setCamInfo(self.calib_data) self.cam_model = PinholeCameraModel() self.cam_model.fromCameraInfo(cam_info) # Initiate maps for rectification self._init_rectify_maps() # Create AprilTag detector object self.at_detector = Detector(families='tag36h11', nthreads=4, quad_decimate=4.0, quad_sigma=0.0, refine_edges=1, decode_sharpening=0.25, debug=0) # Create cv_bridge self.bridge = CvBridge() # Define subscriber to recieve images self.image_sub = rospy.Subscriber( '/' + self.veh + '/camera_node/image/compressed', CompressedImage, self.callback) # Publishers and broadcasters self.pub_robot_pose_tf = rospy.Publisher('~at_baselink_transform', TransformStamped, queue_size=1) self.static_tf_br = tf2_ros.StaticTransformBroadcaster() self.tfBroadcaster = tf.TransformBroadcaster(queue_size=1) self.log(node_name + ' initialized and running')