def __init__(self):
self._datum = rospy.get_param("~datum", None)
if self._datum is not None and len(self._datum) < 3:
self._datum += [0.0]
self._use_datum = self._datum is not None
self._earth_frame_id = rospy.get_param("~earth_frame_id", "earth")
self._utm_frame_id = rospy.get_param("~utm_frame_id", "utm")
self._map_frame_id = rospy.get_param("~map_frame_id", "map")
self._odom_frame_id = rospy.get_param("~odom_frame_id", "odom")
self._body_frame_id = rospy.get_param("~base_frame_id",
"base_link") # currently unused
self._tf_broadcast_rate = rospy.get_param("~broadcast_rate", 10.0)
self._serv = rospy.Service("~set_datum", SetDatum, self._set_datum)
self._tf_buff = Buffer()
TransformListener(self._tf_buff)
self._tf_bcast = TransformBroadcaster()
self._last_datum = None
self._static_map_odom = rospy.get_param("~static_map_odom", False)
self._odom_updated = False
self._update_odom_serv = rospy.Service("~set_odom", Trigger,
self._handle_set_odom)
if not self._use_datum:
rospy.Subscriber("gps_datum", NavSatFix, self._handle_datum)
if not self._static_map_odom:
rospy.Subscriber("waterlinked/pose_with_cov_stamped",
PoseWithCovarianceStamped, self._handle_odom_tf)
else:
StaticTransformBroadcaster().sendTransform(
TransformStamped(
Header(0, rospy.Time.now(), self._map_frame_id),
self._odom_frame_id, None, Quaternion(0, 0, 0, 1)))
self._map_odom_tf = None
rospy.Timer(rospy.Duration.from_sec(1.0 / self._tf_broadcast_rate),
self._send_tf)
def publish_odometry():
# AJC: make this configurable
rate = rospy.Rate(30)
odom_publisher = rospy.Publisher('adabot/odometry', Odometry, queue_size=10)
odom_broadcaster = TransformBroadcaster()
while not rospy.is_shutdown():
current_time = rospy.get_rostime()
# Publish the transform over tf
odom_transform = TransformStamped()
odom_transform.header.stamp = current_time
odom_transform.header.frame_id = 'odom'
odom_transform.child_frame_id = 'base_link'
odom_transform.transform.rotation.w = 1
odom_broadcaster.sendTransform(odom_transform)
# Publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
# odom.pose.pose = 0
odom.twist.twist.linear.x = vx
odom_publisher.publish(odom)
rate.sleep()
class AUVTransformPublisher(Node):
def __init__(self):
super().__init__('auv_transform_publisher')
qos_profile = QoSProfile(depth=10)
self.state_subscriber = self.create_subscription(
PoseStamped, '/triton/state', self.state_callback, 10)
self.path_publisher = self.create_publisher(Path, '/triton/path',
qos_profile)
self.pose_array = []
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.get_logger().info("AUVTransformPublisher successfully started!")
def state_callback(self, msg):
self.pose_array.append(msg)
now = self.get_clock().now()
p = msg.pose.position
q = msg.pose.orientation
odom_trans = TransformStamped()
odom_trans.header.frame_id = msg.header.frame_id
odom_trans.child_frame_id = 'base_link'
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = p.x
odom_trans.transform.translation.y = p.y
odom_trans.transform.translation.z = p.z
odom_trans.transform.rotation = q
self.broadcaster.sendTransform(odom_trans)
path_msg = Path()
path_msg.header.frame_id = msg.header.frame_id
path_msg.header.stamp = now.to_msg()
path_msg.poses = self.pose_array
self.path_publisher.publish(path_msg)
def __init__(self):
self.tf_broadcaster = TransformBroadcaster()
self.ts = TransformStamped()
self.ts.header.frame_id = 'world'
self.ts.child_frame_id = 'drone'
pose_sub = Subscriber('/dji_sdk/pose', PoseStamped, self.pose_callback)
def publish_odom(self):
self.odom_broadcaster = TransformBroadcaster(self)
# self.current_time = datetime.now().microsecond
# compute odometry in a typical way given the velocities of the robot
dt = self.time_step
delta_x = (self.vx * math.cos(self.th) -
self.vy * math.sin(self.th)) * dt
delta_y = (self.vx * math.sin(self.th) +
self.vy * math.cos(self.th)) * dt
delta_th = self.vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
# since all odometry is 6DOF we'll need a quaternion created from yaw
# odom_quat=[0.0,0.0,0.0,1.0]
odom_quat = self.euler_to_quaternion(self.th, 0, 0)
# first, we'll publish the transform over tf
odom_transform = TransformStamped()
odom_transform.header.stamp = self.get_clock().now().to_msg()
# odom_transform.header.stamp = Time(seconds=self.getTime().now()).to_msg()
odom_transform.header.frame_id = 'odom'
odom_transform.child_frame_id = 'base_link'
odom_transform.transform.rotation.x = odom_quat[0]
odom_transform.transform.rotation.y = odom_quat[1]
odom_transform.transform.rotation.z = odom_quat[2]
odom_transform.transform.rotation.w = odom_quat[3]
odom_transform.transform.translation.x = self.x
odom_transform.transform.translation.y = self.y
odom_transform.transform.translation.z = 0.0
# self.get_logger().info('base_link to odom being published : %d' % self.get_clock().now())
self.odom_broadcaster.sendTransform(odom_transform)
odom = Odometry()
odom.header.stamp = self.get_clock().now().to_msg()
# odom.header.stamp = Time(seconds=self.robot.getTime()).to_msg()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
# set the position
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.orientation.x = odom_quat[0]
odom.pose.pose.orientation.y = odom_quat[1]
odom.pose.pose.orientation.z = odom_quat[2]
odom.pose.pose.orientation.w = odom_quat[3]
# set the velocity
odom.twist.twist.linear.x = self.vx
odom.twist.twist.linear.y = self.vy
odom.twist.twist.angular.z = self.vth
# publish the message
self.odom_pub.publish(odom)
def _broadcastTransform(self, Pose, parentName, childName):
br = TransformBroadcaster()
tr = TransformStamped()
tr.header.stamp = Time.now()
tr.header.frame_id = parentName
tr.child_frame_id = childName
tr.transform.translation = Pose.position
tr.transform.rotation = Pose.orientation
br.sendTransform(tr)
def __init__(self):
super().__init__('auv_transform_publisher')
qos_profile = QoSProfile(depth=10)
self.state_subscriber = self.create_subscription(
PoseStamped, '/triton/state', self.state_callback, 10)
self.path_publisher = self.create_publisher(Path, '/triton/path',
qos_profile)
self.pose_array = []
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.get_logger().info("AUVTransformPublisher successfully started!")
def __init__(self):
#############################################################################
super().__init__("odometry_encoders")
self.nodename = self.get_name()
self.get_logger().info("%s started" % self.nodename)
#### parameters #######
self.radius = float(
self.declare_parameter(
'wheels.radius', 0.02569).value) # The wheel radius in meters
self.base_width = float(
self.declare_parameter(
'wheels.base_width',
0.1275).value) # The wheel base width in meters
# the name of the base frame of the robot
self.base_frame_id = self.declare_parameter('base_frame_id',
'base_link').value
# the name of the odometry reference frame
self.odom_frame_id = self.declare_parameter('odom_frame_id',
'odom').value
self.ticks_mode = self.declare_parameter('ticks.ticks_mode',
False).value
# The number of wheel encoder ticks per meter of travel
self.ticks_meter = float(
self.declare_parameter('ticks.ticks_meter', 50.0).value)
self.encoder_min = self.declare_parameter('ticks.encoder_min',
-32768).value
self.encoder_max = self.declare_parameter('ticks.encoder_max',
32768).value
# Init variables
self.init()
# subscriptions / publishers
self.create_subscription(Wheels, "robot/enc_wheels",
self.wheelsCallback,
qos_profile_system_default)
self.create_subscription(Wheels, "robot/enc_ticks_wheels",
self.wheelsEncCallback,
qos_profile_system_default)
self.cal_vel_pub = self.create_publisher(Twist, "cal_vel",
qos_profile_system_default)
self.odom_pub = self.create_publisher(Odometry, "odom",
qos_profile_system_default)
# 5 seconds timer to update parameters
self.create_timer(5, self.parametersCallback)
# TF2 Broadcaster
self.odomBroadcaster = TransformBroadcaster(self)
def __init__(self):
self.fixed_frame = rospy.get_param(
'~fixed_frame', "world"
) # set fixed frame relative to world to apply the transform
self.publish_transform = rospy.get_param(
'~publish_transform',
False) # set true in launch file if the transform is needed
self.pub_jnt = rospy.Publisher("joint_states",
JointState,
queue_size=10) # joint state publisher
self.odom_broadcaster = TransformBroadcaster()
self.jnt_msg = JointState() # joint topic structure
self.odom_trans = TransformStamped()
self.odom_trans.header.frame_id = 'world'
self.odom_trans.child_frame_id = 'base_link'
self.jnt_msg.header = Header()
self.jnt_msg.name = [
'Rev103', 'Rev104', 'Rev105', 'Rev106', 'Rev107', 'Rev108',
'Rev109', 'Rev110', 'Rev111', 'Rev112', 'Rev113', 'Rev114',
'Rev115', 'Rev116', 'Rev117', 'Rev118', 'Rev119', 'Rev120',
'Rev121'
]
"""self.jnt_msg.name = ['Rev105', 'Rev111', 'Rev117', 'Rev104', 'Rev110', 'Rev116',
'Rev103', 'Rev109', 'Rev115', 'Rev108', 'Rev114', 'Rev120',
'Rev107', 'Rev113', 'Rev119', 'Rev106', 'Rev112', 'Rev118',
'Rev121']"""
self.jnt_msg.velocity = []
self.jnt_msg.effort = []
rospy.on_shutdown(self.shutdown_node)
rospy.loginfo("Ready for publishing")
rate = rospy.Rate(50)
while not rospy.is_shutdown():
self.odom_trans.header.stamp = rospy.Time.now()
self.odom_trans.transform.translation.x = 0
self.odom_trans.transform.translation.y = 0
self.odom_trans.transform.translation.z = 0
self.odom_trans.transform.rotation = euler_to_quaternion(
0, 0, 0) # roll,pitch,yaw
self.odom_broadcaster.sendTransform(self.odom_trans)
self.publish_jnt()
rate.sleep()
def callback(msg):
br = TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "odom"
t.child_frame_id = "base_link"
t.transform.translation.x = msg.pose.pose.position.x
t.transform.translation.y = msg.pose.pose.position.y
t.transform.translation.z = 0.0
t.transform.rotation.x = msg.pose.pose.orientation.x
t.transform.rotation.y = msg.pose.pose.orientation.y
t.transform.rotation.z = msg.pose.pose.orientation.z
t.transform.rotation.w = msg.pose.pose.orientation.w
br.sendTransform(t)
def __init__(self):
""" Start up connection to the Neato Robot. """
rospy.init_node('neato')
self.port = rospy.get_param('~port', "/dev/ttyUSB0")
rospy.loginfo("Using port: %s" % (self.port))
self.robot = xv11(self.port)
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.scanPub = rospy.Publisher('base_scan', LaserScan, queue_size=10)
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.cmd_vel = [0, 0]
def __init__(self):
# Read Parameters From YAML File
self.turtlebot_odom_topic = get_param('turtlebot_odom_topic')
self.turtlebot_base_footprint_frame = get_param(
'turtlebot_base_footprint_frame')
self.map_frame = get_param('map_frame')
self.drone_odom_frame = get_param('drone_odom_frame')
self.join_tree_srv_name = get_param('join_tree_srv')
self.debug = get_param('debug')
# TF Broadcaster
self.tf_broadcast = TransformBroadcaster()
# TF Listener
self.tf_listen = Buffer()
self.tf_listener = TransformListener(self.tf_listen)
# Subscribers
Subscriber(self.turtlebot_odom_topic, Odometry, self.turtlebot_odom_cb)
# Services for Joining the Drone TF Tree to Main Tree (Localization)
self.join_tree_service = Service(self.join_tree_srv_name, JointTrees,
self.join_tree_srv_function)
# Parameters
self.drone_tf = TransformStamped()
self.turtlebot_tf = TransformStamped()
self.turtlebot_tf.header.frame_id = self.map_frame
self.turtlebot_tf.child_frame_id = self.turtlebot_base_footprint_frame
self.drone_tf.header.frame_id = self.map_frame
self.drone_tf.child_frame_id = self.drone_odom_frame
self.drone_tf.transform.rotation.w = 1.0
self.turtlebot_tf.transform.rotation.w = 1.0
# Flags
self.join_trees = False # Set True When The Two TF Trees are Joint in One
def __init__(self, handeye_parameters):
self.handeye_parameters = handeye_parameters
# tf structures
self.tfBuffer = Buffer()
"""
used to get transforms to build each sample
:type: tf2_ros.Buffer
"""
self.tfListener = TransformListener(self.tfBuffer)
"""
used to get transforms to build each sample
:type: tf2_ros.TransformListener
"""
self.tfBroadcaster = TransformBroadcaster()
"""
used to publish the calibration after saving it
:type: tf.TransformBroadcaster
"""
# internal input data
self.samples = []
"""
def __init__(self, node_name):
super(DeadReckoningNode, self).__init__(
node_name=node_name,
node_type=NodeType.LOCALIZATION)
self.node_name = node_name
self.veh = rospy.get_param('~veh')
self.publish_hz = rospy.get_param('~publish_hz')
self.encoder_stale_dt = rospy.get_param('~encoder_stale_dt')
self.ticks_per_meter = rospy.get_param('~ticks_per_meter')
self.wheelbase = rospy.get_param('~wheelbase')
self.origin_frame = rospy.get_param('~origin_frame').replace('~', self.veh)
self.target_frame = rospy.get_param('~target_frame').replace('~', self.veh)
self.debug = rospy.get_param('~debug', False)
self.left_encoder_last = None
self.right_encoder_last = None
self.encoders_timestamp_last = None
self.encoders_timestamp_last_local = None
# Current pose, forward velocity, and angular rate
self.timestamp = None
self.x = 0.0
self.y = 0.0
self.z = 0.0
self.yaw = 0.0
self.q = [0.0, 0.0, 0.0, 1.0]
self.tv = 0.0
self.rv = 0.0
# Used for debugging
self.x_trajectory = []
self.y_trajectory = []
self.yaw_trajectory = []
self.time = []
self.total_dist = 0
# Setup subscribers
self.sub_encoder_left = message_filters.Subscriber("~left_wheel", WheelEncoderStamped)
self.sub_encoder_right = message_filters.Subscriber("~right_wheel", WheelEncoderStamped)
# Setup the time synchronizer
self.ts_encoders = message_filters.ApproximateTimeSynchronizer(
[self.sub_encoder_left, self.sub_encoder_right], 10, 0.5)
self.ts_encoders.registerCallback(self.cb_ts_encoders)
# Setup publishers
self.pub = rospy.Publisher('~odom', Odometry, queue_size=10)
# Setup timer
self.timer = rospy.Timer(rospy.Duration(1 / self.publish_hz), self.cb_timer)
self._print_time = 0
self._print_every_sec = 30
# tf broadcaster for odometry TF
self._tf_broadcaster = TransformBroadcaster()
self.loginfo("Initialized")
class TfWrapper(object):
def __init__(self, buffer_size=2):
self.tfBuffer = Buffer(rospy.Duration(buffer_size))
self.tf_listener = TransformListener(self.tfBuffer)
# self.tf_static = StaticTransformBroadcaster()
self.tf_frequency = rospy.Duration(.05)
self.broadcasting_frames = {}
self.broadcasting_frames_lock = Lock()
rospy.sleep(0.1)
def transform_pose(self, target_frame, pose):
try:
transform = self.tfBuffer.lookup_transform(
target_frame,
pose.header.frame_id, # source frame
pose.header.stamp,
rospy.Duration(2.0))
new_pose = do_transform_pose(pose, transform)
return new_pose
except ExtrapolationException as e:
rospy.logwarn(e)
def lookup_transform(self, target_frame, source_frame):
p = PoseStamped()
p.header.frame_id = source_frame
p.pose.orientation.w = 1.0
return self.transform_pose(target_frame, p)
def set_frame_from_pose(self, name, pose_stamped):
with self.broadcasting_frames_lock:
frame = TransformStamped()
frame.header = pose_stamped.header
frame.child_frame_id = name
frame.transform.translation = pose_stamped.pose.position
frame.transform.rotation = pose_stamped.pose.orientation
self.broadcasting_frames[name] = frame
def start_frame_broadcasting(self):
self.tf_broadcaster = TransformBroadcaster()
self.tf_timer = rospy.Timer(self.tf_frequency, self.broadcasting_cb)
def broadcasting_cb(self, _):
with self.broadcasting_frames_lock:
for frame in self.broadcasting_frames.values():
frame.header.stamp = rospy.get_rostime()
self.tf_broadcaster.sendTransform(frame)
class TfBridge(object):
""" Utility class to interface with /tf2 """
def __init__(self):
""" """
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
self.tf_broadcaster = TransformBroadcaster()
def publish_pose_to_tf(self, pose, source_frame, target_frame, time=None):
""" Publish the given pose to /tf2 """
msg = pose.to_msg()
transform = TransformStamped()
transform.child_frame_id = target_frame
transform.header.frame_id = source_frame
if time is not None:
transform.header.stamp = time
else:
transform.header.stamp = rospy.Time().now()
transform.transform.translation = msg.position
transform.transform.rotation = msg.orientation
self.tf_broadcaster.sendTransform(transform)
def get_pose_from_tf(self, source_frame, target_frame, time=None):
""" Get the pose from /tf2 """
try:
if time is not None:
trans = self.tf_buffer.lookup_transform(
source_frame, target_frame, time)
else:
trans = self.tf_buffer.lookup_transform(
source_frame, target_frame, rospy.Time(0))
x = trans.transform.translation.x
y = trans.transform.translation.y
z = trans.transform.translation.z
rx = trans.transform.rotation.x
ry = trans.transform.rotation.y
rz = trans.transform.rotation.z
rw = trans.transform.rotation.w
pose = Vector6D(x=x, y=y, z=z).from_quaternion(rx, ry, rz, rw)
return True, pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logwarn("[perception] Exception occured: {}".format(e))
return False, None
def __init__(self, img_proc=None):
super(LandmarkMethodROS, self).__init__(device_id_facedetection=rospy.get_param("~device_id_facedetection", default="cuda:0"))
self.subject_tracker = FaceEncodingTracker() if rospy.get_param("~use_face_encoding_tracker", default=True) else SequentialTracker()
self.bridge = CvBridge()
self.__subject_bridge = SubjectListBridge()
self.tf2_broadcaster = TransformBroadcaster()
self.tf2_buffer = Buffer()
self.tf2_listener = TransformListener(self.tf2_buffer)
self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
self.visualise_headpose = rospy.get_param("~visualise_headpose", default=True)
self.pose_stabilizers = {} # Introduce scalar stabilizers for pose.
try:
if img_proc is None:
tqdm.write("Wait for camera message")
cam_info = rospy.wait_for_message("/camera_info", CameraInfo, timeout=None)
self.img_proc = PinholeCameraModel()
# noinspection PyTypeChecker
self.img_proc.fromCameraInfo(cam_info)
self.camera_frame = cam_info.header.frame_id
if self.camera_frame.startswith("/"):
self.camera_frame = self.camera_frame[1:]
else:
self.img_proc = img_proc
if np.array_equal(self.img_proc.intrinsicMatrix().A, np.array([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])):
raise Exception('Camera matrix is zero-matrix. Did you calibrate'
'the camera and linked to the yaml file in the launch file?')
tqdm.write("Camera message received")
except rospy.ROSException:
raise Exception("Could not get camera info")
# multiple person images publication
self.subject_pub = rospy.Publisher("/subjects/images", MSG_SubjectImagesList, queue_size=3)
self.headpose_publisher = rospy.Publisher("/subjects/headpose", MSG_HeadposeList, queue_size=3)
self.landmark_publisher = rospy.Publisher("/subjects/landmarks", MSG_LandmarksList, queue_size=3)
# multiple person faces publication for visualisation
self.subject_faces_pub = rospy.Publisher("/subjects/faces", Image, queue_size=3)
Server(ModelSizeConfig, self._dyn_reconfig_callback)
# have the subscriber the last thing that's run to avoid conflicts
self.color_sub = rospy.Subscriber("/image", Image, self.process_image, buff_size=2 ** 24, queue_size=3)
def handle_turtle_pose(msg, turtlename):
transform_broadcaster = TransformBroadcaster()
transformed_msg = TransformStamped()
transformed_msg.header.stamp = rospy.Time.now()
transformed_msg.header.frame_id = 'world'
transformed_msg.child_frame_id = turtlename
transformed_msg.transform.translation.x = msg.x
transformed_msg.transform.translation.y = msg.y
transformed_msg.transform.translation.z = 0.0
q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta)
transformed_msg.transform.rotation.x = q[0]
transformed_msg.transform.rotation.y = q[1]
transformed_msg.transform.rotation.z = q[2]
transformed_msg.transform.rotation.w = q[3]
transform_broadcaster.sendTransform(transformed_msg)
def __init__(self):
rclpy.init()
super().__init__('diff_drive_odometry')
self.odometry = odometry.Odometry()
qos_profile = QoSProfile(depth=10)
self.odomPub = self.create_publisher(Odometry, 'odom', qos_profile)
self.tfPub = TransformBroadcaster(self, qos=qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
self.create_subscription(Int32, "lwheel_ticks", self.leftCallback,
qos_profile)
self.create_subscription(Int32, "rwheel_ticks", self.rightCallback,
qos_profile)
self.create_subscription(PoseWithCovarianceStamped, "initialpose",
self.on_initial_pose, qos_profile)
self.declare_parameters(namespace='',
parameters=[('ticks_per_meter', 1000),
('wheel_separation', 100),
('rate', 10.0),
('base_frame_id', 'base_link'),
('odom_frame_id', 'odom'),
('encoder_min', -32768),
('encoder_max', 32767)])
self.ticksPerMeter = int(self.get_parameter('ticks_per_meter').value)
self.wheelSeparation = float(
self.get_parameter('wheel_separation').value)
self.rate = float(self.get_parameter('rate').value)
self.baseFrameID = self.get_parameter('base_frame_id').value
self.odomFrameID = self.get_parameter('odom_frame_id').value
self.encoderMin = int(self.get_parameter('encoder_min').value)
self.encoderMax = int(self.get_parameter('encoder_max').value)
self.odometry.setWheelSeparation(self.wheelSeparation)
self.odometry.setTicksPerMeter(self.ticksPerMeter)
self.odometry.setEncoderRange(self.encoderMin, self.encoderMax)
self.odometry.setTime(self.get_clock().now())
rate = self.create_rate(self.rate)
while rclpy.ok():
self.publish()
rate.sleep()
def __init__(self):
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
self.tf_broadcaster = TransformBroadcaster()
self.camera_info_topic = rospy.get_param("~camera_info_topic", "")
self.global_frame_id = rospy.get_param("~global_frame_id", "odom")
self.resource_directory = rospy.get_param("~resource_directory", "")
if self.resource_directory == "":
raise ValueError(
"Need to specify the '~resource_directory' parameter")
self.ar_tags_config = rospy.get_param("~ar_tags_config", "")
if self.ar_tags_config == "":
raise ValueError("Need to specify the '~ar_tags_config' parameter")
self.load_config(self.ar_tags_config)
self.tracks = {}
self.camera_info = None
for entity_id, entity in self.config.items():
track = uwds3_msgs.msg.SceneNode()
track.label = self.config[entity_id]["label"]
track.description = self.config[entity_id]["description"]
track.is_located = True
track.has_shape = True
shape = uwds3_msgs.msg.PrimitiveShape()
shape.type = uwds3_msgs.msg.PrimitiveShape.MESH
position = self.config[entity_id]["position"]
orientation = self.config[entity_id]["orientation"]
q = quaternion_from_euler(orientation["x"], orientation["y"],
orientation["z"], "rxyz")
shape.pose.position.x = position["x"]
shape.pose.position.y = position["y"]
shape.pose.position.z = position["z"]
shape.pose.orientation.x = q[0]
shape.pose.orientation.y = q[1]
shape.pose.orientation.z = q[2]
shape.pose.orientation.w = q[3]
shape.mesh_resource = "file://" + self.resource_directory + self.config[
entity_id]["mesh_resource"]
track.shapes = [shape]
self.tracks[entity_id] = track
self.camera_info_subscriber = rospy.Subscriber(
self.camera_info_topic, sensor_msgs.msg.CameraInfo,
self.camera_info_callback)
self.ar_track_subscriber = rospy.Subscriber(
"ar_pose_marker", ar_track_alvar_msgs.msg.AlvarMarkers,
self.observation_callback)
self.tracks_publisher = rospy.Publisher(
"tracks", uwds3_msgs.msg.SceneChangesStamped, queue_size=1)
class Pose2Tf():
def __init__(self):
self.tf_broadcaster = TransformBroadcaster()
self.ts = TransformStamped()
self.ts.header.frame_id = 'world'
self.ts.child_frame_id = 'drone'
pose_sub = Subscriber('/dji_sdk/pose', PoseStamped, self.pose_callback)
def pose_callback(self, msg):
self.ts.header.stamp = Time.now()
self.ts.transform.translation.x = msg.pose.position.x
self.ts.transform.translation.y = msg.pose.position.y
self.ts.transform.translation.z = msg.pose.position.z
self.ts.transform.rotation.x = msg.pose.orientation.x
self.ts.transform.rotation.y = msg.pose.orientation.y
self.ts.transform.rotation.z = msg.pose.orientation.z
self.ts.transform.rotation.w = msg.pose.orientation.w
self.tf_broadcaster.sendTransform(self.ts)
def rosSetup(self):
'''
Creates and inits ROS components
'''
if self.ros_initialized:
return 0
self._transform_listener = TransformListener()
self._transform_broadcaster = TransformBroadcaster()
# Publishers
self._state_publisher = rospy.Publisher('~state',
MarkerMappingState,
queue_size=10)
self._initialpose_publisher = rospy.Publisher(
'/initialpose', PoseWithCovarianceStamped, queue_size=10)
self._global_pose_publisher = rospy.Publisher('/global_pose',
Pose2D,
queue_size=10)
# Subscribers
# topic_name, msg type, callback, queue_size
self.markers_subscriber = rospy.Subscriber('/ar_pose_marker',
AlvarMarkers,
self.markersCb,
queue_size=10)
# Service Servers
self.save_marker_service_server = rospy.Service(
'~save_maker', SaveMarker, self.saveMarkerServiceCb)
self.init_pose_from_marker_service_server = rospy.Service(
'~init_pose_from_marker', InitPoseFromMarker,
self.initPoseFromMarkerServiceCb)
# Service Clients
# self.service_client = rospy.ServiceProxy('service_name', ServiceMsg)
# ret = self.service_client.call(ServiceMsg)
self.ros_initialized = True
self.publishROSstate()
return 0
def __init__(self):
super().__init__('chassis')
self.odom_pub = self.create_publisher(Odometry, "odom_dirty", 10)
self.odom_broadcaster = TransformBroadcaster(self)
self.x = 0
self.y = 0
self.yaw = 0
self.Vx = 0
self.yawRate = 0
self.cmdSpeed = 0
self.cmdSteering = 0
self.tsFeedback = self.get_clock().now().nanoseconds
self.chassis = ChassisInterface()
self.chassis.setWheelCallback(self.processWheelFeedback)
self.sub = self.create_subscription(Twist, 'cmd_vel', self.processCmd,
10)
logging.info('chassis node initialized')
def run(self):
rospy.init_node('ackmn_simulator', anonymous=True)
rospy.Subscriber("/ackmn_drive", AckermannDrive, self.ackmn_callback)
self.pub_twist = rospy.Publisher("/twist_sim", Twist, queue_size=1)
self.br = TransformBroadcaster()
r = rospy.Rate(50)
self.t0 = rospy.get_rostime()
while not rospy.is_shutdown():
self.sim()
self.publish_msgs()
r.sleep()
class WheelOdom(Node):
def __init__(self):
super().__init__('wheel_odom')
self.create_timer(0.1, self.cb)
self.odom_broadcaster = TransformBroadcaster(self)
print('fake odom')
def cb(self):
now = self.get_clock().now()
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = 0.0
quaternion.w = 1.0
transform_stamped_msg = TransformStamped()
transform_stamped_msg.header.stamp = now.to_msg()
transform_stamped_msg.header.frame_id = 'odom'
transform_stamped_msg.child_frame_id = 'base_link'
transform_stamped_msg.transform.translation.x = 0.0
transform_stamped_msg.transform.translation.y = 0.0
transform_stamped_msg.transform.translation.z = 0.0
transform_stamped_msg.transform.rotation.x = quaternion.x
transform_stamped_msg.transform.rotation.y = quaternion.y
transform_stamped_msg.transform.rotation.z = quaternion.z
transform_stamped_msg.transform.rotation.w = quaternion.w
self.odom_broadcaster.sendTransform(transform_stamped_msg)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = 0.0
quaternion.w = 1.0
transform_stamped_msg = TransformStamped()
transform_stamped_msg.header.stamp = now.to_msg()
transform_stamped_msg.header.frame_id = 'map'
transform_stamped_msg.child_frame_id = 'odom'
transform_stamped_msg.transform.translation.x = 0.0
transform_stamped_msg.transform.translation.y = 0.0
transform_stamped_msg.transform.translation.z = 0.0
transform_stamped_msg.transform.rotation.x = quaternion.x
transform_stamped_msg.transform.rotation.y = quaternion.y
transform_stamped_msg.transform.rotation.z = quaternion.z
transform_stamped_msg.transform.rotation.w = quaternion.w
self.odom_broadcaster.sendTransform(transform_stamped_msg)
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = 0.0
quaternion.w = 1.0
transform_stamped_msg = TransformStamped()
transform_stamped_msg.header.stamp = now.to_msg()
transform_stamped_msg.header.frame_id = 'base_link'
transform_stamped_msg.child_frame_id = 'camera_link'
transform_stamped_msg.transform.translation.x = 0.0
transform_stamped_msg.transform.translation.y = 0.0
transform_stamped_msg.transform.translation.z = 0.4
transform_stamped_msg.transform.rotation.x = quaternion.x
transform_stamped_msg.transform.rotation.y = quaternion.y
transform_stamped_msg.transform.rotation.z = quaternion.z
transform_stamped_msg.transform.rotation.w = quaternion.w
self.odom_broadcaster.sendTransform(transform_stamped_msg)
def __init__(self, config, visualize=True, verbose=True, name="one"):
self.visualize = visualize
self.verbose = verbose
# parameters for simulation
self.drive_topic = rospy.get_param("~drive_topic_%s" % name)
self.scan_topic = rospy.get_param("~scan_topic_%s" % name)
self.pose_topic = rospy.get_param("~pose_topic_%s" % name)
self.odom_topic = rospy.get_param("~odom_topic_%s" % name)
self.map_frame = rospy.get_param("~map_frame")
self.base_frame = rospy.get_param("~base_frame")
self.scan_frame = rospy.get_param("~scan_frame")
self.update_pose_rate = rospy.get_param("~update_pose_rate")
self.config = config
# racecar object
self.rcs = RacecarSimulator(config)
# transform broadcaster
self.br = TransformBroadcaster()
# publishers
self.scan_pub = rospy.Publisher(self.scan_topic, LaserScan)
self.odom_pub = rospy.Publisher(self.odom_topic, Odometry)
# subscribers
self.drive_sub = rospy.Subscriber(self.drive_topic,
AckermannDriveStamped,
self.driveCallback,
queue_size=1)
self.pose_sub = rospy.Subscriber(self.pose_topic, PoseStamped,
self.poseCallback)
if self.verbose:
print "Player %s construted!" % name
def __init__(self):
rclpy.init()
super().__init__('oakd_publisher')
qos_profile = QoSProfile(depth=10)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
#Image publisher
#self.pub_depth_img = self.create_publisher(sensor_msgs.msg.Image, "/depth_img", 10)
self.pub_rectified_img = self.create_publisher(Image, "/rectified_img", 10)
# Transform for head declaration
self.transform_head = TransformStamped()
self.transform_head.header.frame_id = 'oak-d_camera_center'
self.transform_head.child_frame_id = 'head'
# Transform for palm declaration
self.transform_palm = TransformStamped()
self.transform_palm.header.frame_id = 'oak-d_camera_center'
self.transform_palm.child_frame_id = 'palm'
#fixed Identity quaternion (no rotation)
self.fps = FPS()
self.advanced_main()
def __init__(self):
rclpy.init()
super().__init__('state_publisher')
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, 'joint_states',
qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
loop_rate = self.create_rate(30)
# robot state
steering_angle = 0.
wheel_angle = 0.
# message declarations
odom_trans = TransformStamped()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'base_link'
joint_state = JointState()
try:
t = 0
while rclpy.ok():
t += 1. / 30.
self.get_logger().info(str(t))
rclpy.spin_once(self)
# Set angle
rotation_names = [
'front_left_wheel_joint', 'front_right_wheel_joint',
'rear_right_wheel_joint', 'rear_left_wheel_joint'
]
wheel_angle += 2 * pi * t / 50
wheel_angle = atan2(sin(wheel_angle), cos(wheel_angle))
rotation_position = [
wheel_angle, wheel_angle, wheel_angle, wheel_angle
]
# Set steering
steering_angle = pi / 3. * sin(t * 2 * pi / 4.)
steering_names = [
'front_left_steering_joint', 'front_right_steering_joint'
]
steering_position = [steering_angle, steering_angle]
# update joint_state
now = self.get_clock().now()
joint_state.header.stamp = now.to_msg()
joint_state.name = rotation_names + steering_names
joint_state.position = rotation_position + steering_position
# update transform
# (moving in a circle with radius=2)
angle = t * 2 * pi / 10.
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = cos(angle) * 2
odom_trans.transform.translation.y = sin(angle) * 2
odom_trans.transform.translation.z = 0.7
odom_trans.transform.rotation = \
euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw
# send the joint state and transform
self.joint_pub.publish(joint_state)
self.broadcaster.sendTransform(odom_trans)
# This will adjust as needed per iteration
loop_rate.sleep()
except KeyboardInterrupt:
pass
class ChassisNode(Node):
CameraElevationMeters = 0.4
WheelBaselineMeters = 0.430
WheelRadiusMeters = 0.115
ReportIntervalSec = 0.02
SecInNsec = 10**-9
RadPerSecInWheelFeedback = 1.
MaxSpeedMetersSec = 1.0 # linear velocity from vel_cmd is scaled and bounded to this
MaxYawRateRadSec = 1.0 # yaw rate from vel_cmd is scaled and bounded to this. todo : set to actual max yaw rate of chassis
def __init__(self):
super().__init__('chassis')
self.odom_pub = self.create_publisher(Odometry, "odom_dirty", 10)
self.odom_broadcaster = TransformBroadcaster(self)
self.x = 0
self.y = 0
self.yaw = 0
self.Vx = 0
self.yawRate = 0
self.cmdSpeed = 0
self.cmdSteering = 0
self.tsFeedback = self.get_clock().now().nanoseconds
self.chassis = ChassisInterface()
self.chassis.setWheelCallback(self.processWheelFeedback)
self.sub = self.create_subscription(Twist, 'cmd_vel', self.processCmd,
10)
logging.info('chassis node initialized')
def processWheelFeedback(self, DeviceIdIgnored, respTuple):
now = self.get_clock().now()
deltaTimeSec = (now.nanoseconds -
self.tsFeedback) * ChassisNode.SecInNsec
zeroRotation = Quaternion()
zeroTranslation = Vector3()
zeroTranslation.x = 0.0
zeroTranslation.y = 0.0
zeroTranslation.z = 0.0
if deltaTimeSec >= ChassisNode.ReportIntervalSec:
self.tsFeedback = now.nanoseconds
wheelLRadSec = respTuple[0] * ChassisNode.RadPerSecInWheelFeedback
wheelRRadSec = respTuple[1] * ChassisNode.RadPerSecInWheelFeedback
self.calculatePose(deltaTimeSec, wheelRRadSec, wheelLRadSec)
logging.debug(
'chassis wheel feedback : {0} {1} speed {2} yawRate {3} pos [{4} {5}] yaw {6}'
.format(wheelLRadSec, wheelRRadSec, self.Vx, self.yawRate,
self.x, self.y, self.yaw))
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.yaw / 2)
quaternion.w = cos(self.yaw / 2)
odom = Odometry()
odom.header.stamp = now.to_msg()
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
odom.pose.covariance[0] = 0.1
odom.pose.covariance[7] = 0.1
odom.pose.covariance[35] = 0.1
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = self.Vx
odom.twist.twist.linear.y = 0.0
odom.twist.twist.angular.z = self.yawRate
odom.twist.covariance[0] = 0.01
odom.twist.covariance[7] = 0.01
odom.twist.covariance[35] = 0.01
self.odom_pub.publish(odom)
transform_stamped_msg = TransformStamped()
transform_stamped_msg.header.stamp = now.to_msg()
transform_stamped_msg.header.frame_id = 'odom'
transform_stamped_msg.child_frame_id = 'base_link'
transform_stamped_msg.transform.translation = zeroTranslation
transform_stamped_msg.transform.translation.x = self.x
transform_stamped_msg.transform.translation.y = self.y
transform_stamped_msg.transform.rotation = quaternion
self.odom_broadcaster.sendTransform(transform_stamped_msg)
transform_stamped_msg = TransformStamped()
transform_stamped_msg.header.stamp = now.to_msg()
transform_stamped_msg.header.frame_id = 'map'
transform_stamped_msg.child_frame_id = 'odom'
transform_stamped_msg.transform.translation = zeroTranslation
transform_stamped_msg.transform.rotation = zeroRotation
self.odom_broadcaster.sendTransform(transform_stamped_msg)
transform_stamped_msg = TransformStamped()
transform_stamped_msg.header.stamp = now.to_msg()
transform_stamped_msg.header.frame_id = 'base_link'
transform_stamped_msg.child_frame_id = 'camera_link'
transform_stamped_msg.transform.translation = zeroTranslation
transform_stamped_msg.transform.translation.z = ChassisNode.CameraElevationMeters
self.odom_broadcaster.sendTransform(transform_stamped_msg)
def processCmd(self, data):
#todo : add deadzone for Vx and yaw rate
#todo : reset chassis control to zeroes if cmd_vel has expired
cmdSpeedBounded = max(min(data.linear.x, MaxSpeedMetersSec),
-MaxSpeedMetersSec)
self.cmdSpeed = cmdSpeedBounded / MaxSpeedMetersSec
self.chassis.setSpeed(self.cmdSpeed)
cmdYawRateBounded = max(min(data.angular.z, MaxYawRateRadSec),
-MaxYawRateRadSec)
self.cmdSteering = cmdYawRateBounded / MaxYawRateRadSec
self.chassis.setSteering(self.cmdSteering)
logging.debug('chassis cmd speed {0}/{1} steering {2}/{3}'.format(
data.linear.x, self.cmdSpeed, data.angular.z, cmdSteering))
#dtSec : integration interval
#Lvel, Rvel : wheels angular speed, rad/sec
def calculatePose(self, dtSec, Lvel, Rvel):
# Lvel= -Lvel
self.Vx = (ChassisNode.WheelRadiusMeters) * (Rvel + Lvel) / 2
self.yawRate = -1.6 * (ChassisNode.WheelRadiusMeters) * (
Rvel - Lvel) / ChassisNode.WheelBaselineMeters
self.yaw += dtSec * self.yawRate
self.y += dtSec * sin(self.yaw) * self.Vx
self.x += dtSec * cos(self.yaw) * self.Vx
def read_raw(self):
if self.__count > 0:
self.__data = np.multiply(self.__temp, 1.0 / self.__count)
norm = np.linalg.norm(self.__data)
self.__data /= norm
self.__count = 0
self.__temp = np.zeros((4, ))
return self.__data
def setOffset(self, offset):
offset[3] = -offset[3]
self.__offset_inv = np.array(offset, dtype=np.float64)
self.__q_offset = quaternion_multiply(self.__ref, self.__offset_inv)
br = TransformBroadcaster()
def sendTF(frame_id, child_frame_id, xyz=[0, 0, 0], q=[0, 0, 0, 1]):
tf = TransformStamped()
tf.header.stamp = rospy.Time.now()
tf.header.frame_id = frame_id
tf.child_frame_id = child_frame_id
tf.transform.translation = Vector3(xyz[0], xyz[1], xyz[2])
tf.transform.rotation = Quaternion(q[0], q[1], q[2], q[3])
br.sendTransform(tf)
JR1 = IMU(ref=quaternion_from_euler(0, math.pi / 2, 0))
JR2 = IMU(ref=quaternion_from_euler(math.pi / 2, 0, math.pi / 2))
JR3 = IMU(ref=quaternion_from_euler(math.pi / 2, 0, math.pi / 2))
