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