def __init__(self,
parent_frame_id: str = "world",
child_frame_id: str = "unknown_child_id",
use_sim_time: bool = True,
node_name: str = 'drl_grasping_camera_tf_broadcaster'):
try:
rclpy.init()
except:
if not rclpy.ok():
import sys
sys.exit("ROS 2 could not be initialised")
Node.__init__(self, node_name)
self.set_parameters([
Parameter('use_sim_time',
type_=Parameter.Type.BOOL,
value=use_sim_time)
])
qos = QoSProfile(durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_ALL)
self._tf2_broadcaster = StaticTransformBroadcaster(self, qos=qos)
self._transform_stamped = TransformStamped()
self.set_parent_frame_id(parent_frame_id)
self.set_child_frame_id(child_frame_id)
def __init__(self, node, device_key, wb_device, params=None):
super().__init__(node, device_key, wb_device, params)
self.__publishers = {}
self.__static_transforms = []
self.__static_broadcaster = None
self.__noise = self._get_param('noise', 1e-2)
# Exit if disabled
if self._disable:
return
# Change default timestep
self._timestep = 128
# Create topics
if wb_device.getNumberOfLayers() > 1:
wb_device.enablePointCloud()
self.__publisher = node.create_publisher(PointCloud2,
self._topic_name, 1)
else:
self.__publisher = node.create_publisher(LaserScan,
self._topic_name, 1)
self.__static_broadcaster = StaticTransformBroadcaster(node)
transform_stamped = TransformStamped()
transform_stamped.header.frame_id = self._frame_id
transform_stamped.child_frame_id = self._frame_id + '_rotated'
transform_stamped.transform.rotation.x = 0.5
transform_stamped.transform.rotation.y = 0.5
transform_stamped.transform.rotation.z = -0.5
transform_stamped.transform.rotation.w = 0.5
self.__static_broadcaster.sendTransform(transform_stamped)
def __init__(self, node, device_key, wb_device, params=None):
super().__init__(node, device_key, wb_device, params)
self.__base_footprint_broadcaster = None
# Determine default params
params = params or {}
self.__publish_robot_description = self._get_param(
'publish_robot_description', True)
self.__publish_base_footprint = self._get_param(
'publish_base_footprint', False)
# Create robot_description publishers if needed
if self.__publish_robot_description:
urdf = self._wb_device.getUrdf(self.__get_urdf_prefix())
self.__save_urdf_to_file(urdf)
self.__set_string_param('robot_state_publisher',
'robot_description', urdf)
if self.__publish_base_footprint:
self.__base_footprint_broadcaster = StaticTransformBroadcaster(
self._node)
transform_stamped = TransformStamped()
transform_stamped.header.frame_id = self.__get_urdf_prefix(
) + 'base_link'
transform_stamped.child_frame_id = self.__get_urdf_prefix(
) + 'base_footprint'
transform_stamped.transform.rotation.w = 1.0
self.__base_footprint_broadcaster.sendTransform(transform_stamped)
def __init__(self, name):
super().__init__(name)
fill_map_param = self.declare_parameter('fill_map', True)
# Init map related elements
self.map = [-1] * MAP_WIDTH * MAP_HEIGHT
self.map_publisher = self.create_publisher(
OccupancyGrid,
'/map',
qos_profile=QoSProfile(
depth=1,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
))
self.tf_publisher = StaticTransformBroadcaster(self)
tf = TransformStamped()
tf.header.stamp = self.get_clock().now().to_msg()
tf.header.frame_id = 'map'
tf.child_frame_id = 'odom'
tf.transform.translation.x = 0.0
tf.transform.translation.y = 0.0
tf.transform.translation.z = 0.0
self.tf_publisher.sendTransform(tf)
# Init laser related elements
if fill_map_param.value:
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.scanner_subscriber = self.create_subscription(
LaserScan, '/scan', self.update_map, 1)
# Start publishing the map
self.publish_map()
self.create_timer(1, self.publish_map)
def __init__(self, buffer_size=2):
self.tfBuffer = Buffer(rospy.Duration(buffer_size))
self.tf_listener = TransformListener(self.tfBuffer)
self.tf_static = StaticTransformBroadcaster()
self.tf_frequency = rospy.Duration(1.0)
self.broadcasting_frames = []
self.broadcasting_frames_lock = Lock()
rospy.sleep(0.1)
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(1.0)
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 add_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.append(frame)
def start_frame_broadcasting(self):
self.tf_broadcaster = tf.TransformBroadcaster()
self.tf_timer = rospy.Timer(self.tf_frequency, self.broadcasting_cb)
def broadcasting_cb(self, data):
with self.broadcasting_frames_lock:
for frame in self.broadcasting_frames:
frame.header.stamp = rospy.get_rostime()
self.tf_broadcaster.sendTransformMessage(frame)
def broadcast_static_frame(self, name, pose_stamped):
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.tf_static.sendTransform(frame)
def __init__(self, cowork=False):
self.stl_pkg_dir = r_path
self.grasping_pose = grasp_poses.grasping_pose
self.br = StaticTransformBroadcaster()
if cowork:
moveit_commander.roscpp_initialize(sys.argv)
self.scene = moveit_commander.PlanningSceneInterface()
self.client_for_HoleCheck = rospy.ServiceProxy(
"/to_HoleCheck", asm_Srv)
self.client_for_RobotControl = rospy.ServiceProxy(
"/to_RobotControl", asm_Srv)
#self.wait_for_services()
else:
pass
def __init__(self):
self._datum = rospy.get_param("~datum", None)
if self._datum is not None and len(self._datum) < 3:
self._datum += [0.0]
self._use_datum = self._datum is not None
self._earth_frame_id = rospy.get_param("~earth_frame_id", "earth")
self._utm_frame_id = rospy.get_param("~utm_frame_id", "utm")
self._map_frame_id = rospy.get_param("~map_frame_id", "map")
self._odom_frame_id = rospy.get_param("~odom_frame_id", "odom")
self._body_frame_id = rospy.get_param("~base_frame_id",
"base_link") # currently unused
self._tf_broadcast_rate = rospy.get_param("~broadcast_rate", 10.0)
self._serv = rospy.Service("~set_datum", SetDatum, self._set_datum)
self._tf_buff = Buffer()
TransformListener(self._tf_buff)
self._tf_bcast = TransformBroadcaster()
self._last_datum = None
self._static_map_odom = rospy.get_param("~static_map_odom", False)
self._odom_updated = False
self._update_odom_serv = rospy.Service("~set_odom", Trigger,
self._handle_set_odom)
if not self._use_datum:
rospy.Subscriber("gps_datum", NavSatFix, self._handle_datum)
if not self._static_map_odom:
rospy.Subscriber("waterlinked/pose_with_cov_stamped",
PoseWithCovarianceStamped, self._handle_odom_tf)
else:
StaticTransformBroadcaster().sendTransform(
TransformStamped(
Header(0, rospy.Time.now(), self._map_frame_id),
self._odom_frame_id, None, Quaternion(0, 0, 0, 1)))
self._map_odom_tf = None
rospy.Timer(rospy.Duration.from_sec(1.0 / self._tf_broadcast_rate),
self._send_tf)
def __init__(self, cowork=False):
self.stl_pkg_path = r.get_path("object_description")
self.grasping_pose = grasp_poses.grasping_pose
self.br = StaticTransformBroadcaster()
if cowork:
moveit_commander.roscpp_initialize(sys.argv)
self.scene = moveit_commander.PlanningSceneInterface()
self.client_for_robot = rospy.ServiceProxy("/to_HoleCheck",
asm_Srv)
self.client_asm_check = rospy.ServiceProxy("/to_RobotControl",
asm_Srv)
rospy.wait_for_service("/to_HoleCheck")
rospy.wait_for_service("/to_RobotControl")
else:
pass
def run(self):
rosRate = Rate(30)
broadcaster = StaticTransformBroadcaster()
while not is_shutdown():
rot = Rotation(self._rotMatrixArray[0], self._rotMatrixArray[1],
self._rotMatrixArray[2], self._rotMatrixArray[3],
self._rotMatrixArray[4], self._rotMatrixArray[5],
self._rotMatrixArray[6], self._rotMatrixArray[7],
self._rotMatrixArray[8])
quat = rot.GetQuaternion()
staticTransform = self._setTransform(self._robotBaseFrame,
self._HDFrame, quat)
broadcaster.sendTransform(staticTransform)
rosRate.sleep()
def __init__(self):
self.__tf_buffer = Buffer()
self.__tf_listener = TransformListener(self.__tf_buffer)
self.__static_broadcaster = StaticTransformBroadcaster()
self.__tool_transform = self.empty_transform()
self.__tcp_transform = self.empty_transform()
self.__enable_tcp = False
# Publisher
self.__tcp_publisher = rospy.Publisher('~tcp',
TCP,
queue_size=10,
latch=True)
rospy.Timer(rospy.Duration.from_sec(0.5), self.__send_tcp_transform)
# Services
rospy.Service('~set_tcp', SetTCP, self.__callback_set_tcp)
rospy.Service('~reset_tcp', Trigger, self.__callback_reset_tcp)
rospy.Service('~enable_tcp', SetBool, self.__callback_enable_tcp)
def main():
pose = TransformStamped()
static_br = StaticTransformBroadcaster()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "world"
pose.child_frame_id = "robot"
pose.transform.translation.x = args['x']
pose.transform.translation.y = args['y']
pose.transform.translation.z = args['z']
quat = tf.transformations.quaternion_from_euler(
args['roll'],args['pitch'],args['yaw'])
pose.transform.rotation.x = quat[0]
pose.transform.rotation.y = quat[1]
pose.transform.rotation.z = quat[2]
pose.transform.rotation.w = quat[3]
static_br.sendTransform(pose)
rospy.spin()
def publish(self, R_q, t):
'''Publish static transform for base_footprint to camera_pose
This can be published via the command-line too'''
broadcaster = StaticTransformBroadcaster() # From tf2_ros
static_tf = TransformStamped() # A message from geometry_msgs.msg
static_tf.header.stamp = rospy.Time.now()
static_tf.header.frame_id = "base_footprint"
static_tf.child_frame_id = "camera_pose"
static_tf.transform.translation.x = t[0]
static_tf.transform.translation.y = t[1]
static_tf.transform.translation.z = t[2]
static_tf.transform.rotation.x = R_q[0]
static_tf.transform.rotation.y = R_q[1]
static_tf.transform.rotation.z = R_q[2]
static_tf.transform.rotation.w = R_q[3]
broadcaster.sendTransform(static_tf)
rospy.loginfo("Broadcasting static transform")
def __init__(self, args):
super().__init__('epuck_driver',
args,
wheel_distance=DEFAULT_WHEEL_DISTANCE,
wheel_radius=DEFAULT_WHEEL_RADIUS)
self.start_device_manager(DEVICE_CONFIG)
# Intialize distance sensors for LaserScan topic
self.distance_sensors = {}
for i in range(NB_INFRARED_SENSORS):
sensor = self.robot.getDistanceSensor('ps{}'.format(i))
sensor.enable(self.timestep)
self.distance_sensors['ps{}'.format(i)] = sensor
self.laser_publisher = self.create_publisher(LaserScan, '/scan', 1)
self.tof_sensor = self.robot.getDistanceSensor('tof')
if self.tof_sensor:
self.tof_sensor.enable(self.timestep)
else:
self.get_logger().info(
'ToF sensor is not present for this e-puck version')
laser_transform = TransformStamped()
laser_transform.header.stamp = Time(
seconds=self.robot.getTime()).to_msg()
laser_transform.header.frame_id = 'base_link'
laser_transform.child_frame_id = 'laser_scanner'
laser_transform.transform.rotation.x = 0.0
laser_transform.transform.rotation.y = 0.0
laser_transform.transform.rotation.z = 0.0
laser_transform.transform.rotation.w = 1.0
laser_transform.transform.translation.x = 0.0
laser_transform.transform.translation.y = 0.0
laser_transform.transform.translation.z = 0.033
self.static_broadcaster = StaticTransformBroadcaster(self)
self.static_broadcaster.sendTransform(laser_transform)
# Main loop
self.create_timer(self.timestep / 1000, self.__publish_laserscan_data)
def __init__(self):
"""Init Localisation node"""
super().__init__("localisation_node")
self.robot = self.get_namespace().strip("/")
self.side = self.declare_parameter("side", "blue")
self.add_on_set_parameters_callback(self._on_set_parameters)
self.robot_pose = PoseStamped()
(
self.robot_pose.pose.position.x,
self.robot_pose.pose.position.y,
theta,
) = self.fetchStartPosition()
self.robot_pose.pose.orientation = euler_to_quaternion(theta)
self._tf_brodcaster = StaticTransformBroadcaster(self)
self._tf = TransformStamped()
self._tf.header.frame_id = "map"
self._tf.child_frame_id = "odom"
self.update_transform()
self.subscription_ = self.create_subscription(
MarkerArray,
"/allies_positions_markers",
self.allies_subscription_callback,
10,
)
self.subscription_ = self.create_subscription(
Odometry,
"odom",
self.odom_callback,
10,
)
self.tf_publisher_ = self.create_publisher(
TransformStamped, "adjust_odometry", 10
)
self.odom_map_relative_publisher_ = self.create_publisher(
PoseStamped, "odom_map_relative", 10
)
self.last_odom_update = 0
self.get_logger().info(f"Default side is {self.side.value}")
self.vlx = VlxReadjustment(self)
self.get_logger().info("Localisation node is ready")
def __init__(self, args):
super().__init__(
'khepera_iv_driver',
args,
wheel_distance=0.1054,
wheel_radius=0.021
)
self.start_device_manager(DEVICE_CONFIG)
# Intialize distance sensors for LaserScan topic
self.distance_sensors = {}
for name in DISTANCE_SENSOR_ANGLE.keys():
sensor = self.robot.getDistanceSensor(name)
sensor.enable(self.timestep)
self.distance_sensors[name] = sensor
for name in ULTRASONIC_SENSOR_ANGLE.keys():
sensor = self.robot.getDistanceSensor(name)
sensor.enable(self.timestep)
self.distance_sensors[name] = sensor
self.laser_publisher = self.create_publisher(LaserScan, '/scan', 1)
laser_transform = TransformStamped()
laser_transform.header.stamp = Time(seconds=self.robot.getTime()).to_msg()
laser_transform.header.frame_id = 'base_link'
laser_transform.child_frame_id = 'laser_scanner'
laser_transform.transform.rotation.x = 0.0
laser_transform.transform.rotation.y = 0.0
laser_transform.transform.rotation.z = 0.0
laser_transform.transform.rotation.w = 1.0
laser_transform.transform.translation.x = 0.0
laser_transform.transform.translation.y = 0.0
laser_transform.transform.translation.z = 0.033
self.static_broadcaster = StaticTransformBroadcaster(self)
self.static_broadcaster.sendTransform(laser_transform)
# Main loop
self.create_timer(self.timestep / 1000, self.__publish_laserscan_data)
def draw_aruco_map():
'''
arucotag 绘制aruco地图
'''
global rivz_pub
global tf_staic_broadcast
rospy.init_node('draw_aruco_map', anonymous=True)
tf_staic_broadcast = StaticTransformBroadcaster()
rivz_pub = rospy.Publisher('aruco_map_rviz', Marker, queue_size=10)
while(rivz_pub.get_num_connections() < 1):
if rospy.is_shutdown():
rospy.signal_shutdown('Master is not on')
# load_aruco_map()
rospy.logwarn("请创建RVIZ的Subscriber")
rospy.sleep(5)
load_aruco_map()
rospy.signal_shutdown('Quit')
import rospy
from tf2_ros import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
rospy.init_node('pubpose')
counter = defaultdict(int)
L = []
with open(sys.argv[1]) as f:
for line in f:
sign, tx, ty, tz, qx, qy, qz, qw = line.strip().split('\t')
trans = TransformStamped()
trans.header.frame_id = 'map'
trans.child_frame_id = '%s%d' % (sign, counter[sign])
counter[sign] += 1
trans.transform.translation.x = float(tx)
trans.transform.translation.y = float(ty)
trans.transform.translation.z = float(tz)
trans.transform.rotation.x = float(qx)
trans.transform.rotation.y = float(qy)
trans.transform.rotation.z = float(qz)
trans.transform.rotation.w = float(qw)
L.append(trans)
stb = StaticTransformBroadcaster()
stb.sendTransform(L)
print(L)
rospy.spin()
class RobotDevice(Device):
"""
ROS2 wrapper for Webots Robot node.
Creates suitable ROS2 interface based on Webots [Robot](https://cyberbotics.com/doc/reference/robot) node.
It allows the following functinalities:
- Updates `robot_description` parameter of `robot_state_publisher` node based on obtained URDF.
Args:
node (WebotsNode): The ROS2 node.
device_key (str): Unique identifier of the device used for configuration.
wb_device (Robot): Webots node of type Robot.
Kwargs:
params (dict): Dictionary with configuration options in format of::
dict: {
'publish_robot_description': bool, # Whether to publish robot description (default True)
'publish_base_footprint': bool, # Whether to publish `base_footprint` link (default False)
}
"""
def __init__(self, node, device_key, wb_device, params=None):
super().__init__(node, device_key, wb_device, params)
self.__base_footprint_broadcaster = None
# Determine default params
params = params or {}
self.__publish_robot_description = self._get_param('publish_robot_description', True)
self.__publish_base_footprint = self._get_param('publish_base_footprint', False)
# Create robot_description publishers if needed
if self.__publish_robot_description:
urdf = self._wb_device.getUrdf(self.__get_urdf_prefix())
self.__save_urdf_to_file(urdf)
self.__set_string_param('robot_state_publisher', 'robot_description', urdf)
if self.__publish_base_footprint:
self.__base_footprint_broadcaster = StaticTransformBroadcaster(self._node)
transform_stamped = TransformStamped()
transform_stamped.header.frame_id = self.__get_urdf_prefix() + 'base_link'
transform_stamped.child_frame_id = self.__get_urdf_prefix() + 'base_footprint'
transform_stamped.transform.rotation.w = 1.0
self.__base_footprint_broadcaster.sendTransform(transform_stamped)
def __save_urdf_to_file(self, urdf):
"""Write URDF to a file for debugging purposes."""
filename = 'webots_robot_{}.urdf'.format(self._node.robot.getName().replace(' ', '_').replace('/', '_'))
with open(os.path.join(tempfile.gettempdir(), filename), 'w') as urdf_file:
urdf_file.write(urdf)
def __set_string_param(self, node, name, value):
self.cli = self._node.create_client(SetParameters, self._node.get_namespace() + node + '/set_parameters')
self.cli.wait_for_service(timeout_sec=1)
req = SetParameters.Request()
param_value = ParameterValue(string_value=value, type=ParameterType.PARAMETER_STRING)
param = Parameter(name=name, value=param_value)
req.parameters.append(param)
self.cli.call_async(req)
def __get_urdf_prefix(self):
return self._node.get_namespace()[1:].replace('/', '_')
def step(self):
pass
def send_aruco_static_transform(aruco_id, aruco_pose):
'''
发布静态Aruco的TF变换
'''
global tf_staic_broadcast # 静态广播
transform_stamped = TransformStamped()
transform_stamped.header.stamp = rospy.Time.now()
transform_stamped.header.frame_id = 'world'
transform_stamped.child_frame_id = 'aruco_%d'%aruco_id
transform_stamped.transform.translation = aruco_pose.position
transform_stamped.transform.rotation = aruco_pose.orientation
tf_staic_broadcast.sendTransform(transform_stamped)
def finish_sample_collect_callback(info):
rospy.loginfo(info) # 打印
load_arucos_dict() # 载入数据
draw_raw_map() # 绘制原始地图 并退出节点
if __name__=="__main__":
rospy.init_node('build_aruco_map', anonymous=True) # 初始化节点
BASE_ARUCO_ID = rospy.get_param('/build_aruco_map_from_log/base_aruco_id') # 获取基准参考码的ID
tf_staic_broadcast = StaticTransformBroadcaster() # 静态TF变换发布
rviz_marker_pub = rospy.Publisher('draw_aruco_map', Marker, queue_size=4000)
# 监听样本采集完成
sample_done_sub = rospy.Subscriber('/aruco_sample_done', String, finish_sample_collect_callback)
rospy.spin()
class SimpleMapper(Node):
def __init__(self, name):
super().__init__(name)
fill_map_param = self.declare_parameter('fill_map', True)
# Init map related elements
self.map = [-1] * MAP_WIDTH * MAP_HEIGHT
self.map_publisher = self.create_publisher(
OccupancyGrid,
'/map',
qos_profile=QoSProfile(
depth=1,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
))
self.tf_publisher = StaticTransformBroadcaster(self)
tf = TransformStamped()
tf.header.stamp = self.get_clock().now().to_msg()
tf.header.frame_id = 'map'
tf.child_frame_id = 'odom'
tf.transform.translation.x = 0.0
tf.transform.translation.y = 0.0
tf.transform.translation.z = 0.0
self.tf_publisher.sendTransform(tf)
# Init laser related elements
if fill_map_param.value:
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.scanner_subscriber = self.create_subscription(
LaserScan, '/scan', self.update_map, 1)
# Start publishing the map
self.publish_map()
self.create_timer(1, self.publish_map)
def publish_map(self):
now = self.get_clock().now()
msg = OccupancyGrid()
msg.header.stamp = now.to_msg()
msg.header.frame_id = 'map'
msg.info.resolution = RESOLUTION
msg.info.width = MAP_WIDTH
msg.info.height = MAP_HEIGHT
msg.info.origin.position.x = WORLD_ORIGIN_X
msg.info.origin.position.y = WORLD_ORIGIN_Y
msg.data = self.map
self.map_publisher.publish(msg)
def update_map(self, msg):
# Determine transformation of laser and robot in respect to odometry
laser_rotation = None
laser_translation = None
try:
tf = self.tf_buffer.lookup_transform('odom', msg.header.frame_id,
Time(sec=0, nanosec=0))
laser_rotation = quaternion_to_euler(tf.transform.rotation)[0]
laser_translation = tf.transform.translation
except (LookupException, ConnectivityException,
ExtrapolationException) as e:
print('No required transformation found: `{}`'.format(str(e)))
return
# Determine position of robot and laser
world_robot_x = laser_translation.x + WORLD_ORIGIN_X
world_robot_y = laser_translation.y + WORLD_ORIGIN_Y
world_laser_xs = []
world_laser_ys = []
laser_range_angle = msg.angle_min + laser_rotation
for laser_range in msg.ranges:
if laser_range < msg.range_max and laser_range > msg.range_min:
laser_x = world_robot_x + laser_range * cos(laser_range_angle)
laser_y = world_robot_y + laser_range * sin(laser_range_angle)
world_laser_xs.append(laser_x)
world_laser_ys.append(laser_y)
laser_range_angle += msg.angle_increment
# Determine position on map (from world coordinates)
robot_x = int(world_robot_x / RESOLUTION)
robot_y = int(world_robot_y / RESOLUTION)
laser_xs = []
laser_ys = []
for world_laser_x, world_laser_y in zip(world_laser_xs,
world_laser_ys):
laser_x = int(world_laser_x / RESOLUTION)
laser_y = int(world_laser_y / RESOLUTION)
laser_xs.append(laser_x)
laser_ys.append(laser_y)
# Fill the map based on known readings
for laser_x, laser_y in zip(laser_xs, laser_ys):
self.plot_bresenham_line(robot_x, laser_x, robot_y, laser_y)
self.map[laser_y * MAP_WIDTH + laser_x] = 100
def plot_bresenham_line(self, x0, x1, y0, y1):
# Bresenham's line algorithm (https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm)
dx = abs(x1 - x0)
sx = 1 if x0 < x1 else -1
dy = -abs(y1 - y0)
sy = 1 if y0 < y1 else -1
err = dx + dy
while True:
self.map[y0 * MAP_WIDTH + x0] = 0
if x0 == x1 and y0 == y1:
break
e2 = 2 * err
if e2 >= dy:
err += dy
x0 += sx
if e2 <= dx:
err += dx
y0 += sy
class ArmTCPTransformHandler:
"""
This class uses a TransformListener to handle transforms related to the TCP.
"""
def __init__(self):
self.__tf_buffer = Buffer()
self.__tf_listener = TransformListener(self.__tf_buffer)
self.__static_broadcaster = StaticTransformBroadcaster()
def ee_link_to_tcp_pose_target(self, pose, ee_link):
try:
transform_tcp_to_ee_link = self.__tf_buffer.lookup_transform(
ee_link, "TCP", rospy.Time(0))
transform_tcp_to_ee_link.header.frame_id = "ee_link_target"
transform_tcp_to_ee_link.child_frame_id = "tcp_target"
stamp = transform_tcp_to_ee_link.header.stamp
transform_world_to_tcp_target = self.transform_from_pose(
pose, "base_link", "ee_link_target", stamp)
self.__tf_buffer.set_transform(transform_world_to_tcp_target,
"default_authority")
self.__tf_buffer.set_transform(transform_tcp_to_ee_link,
"default_authority")
ee_link_target_transform = self.__tf_buffer.lookup_transform(
"base_link", "tcp_target", stamp)
return self.pose_from_transform(ee_link_target_transform.transform)
except ArmTCPTransformHandlerException:
self.set_empty_tcp_to_ee_link_transform(ee_link)
return pose
def tcp_to_ee_link_pose_target(self, pose, ee_link):
try:
transform_tcp_to_ee_link = self.__tf_buffer.lookup_transform(
"TCP", ee_link, rospy.Time(0))
transform_tcp_to_ee_link.header.frame_id = "tcp_target"
transform_tcp_to_ee_link.child_frame_id = "ee_link_target"
stamp = transform_tcp_to_ee_link.header.stamp
transform_world_to_tcp_target = self.transform_from_pose(
pose, "base_link", "tcp_target", stamp)
self.__tf_buffer.set_transform(transform_world_to_tcp_target,
"default_authority")
self.__tf_buffer.set_transform(transform_tcp_to_ee_link,
"default_authority")
ee_link_target_transform = self.__tf_buffer.lookup_transform(
"base_link", "ee_link_target", stamp)
return self.pose_from_transform(ee_link_target_transform.transform)
except ArmTCPTransformHandlerException:
self.set_empty_tcp_to_ee_link_transform(ee_link)
return pose
def tcp_to_ee_link_position_target(self, position, ee_link):
pose = Pose(position, Quaternion(0, 0, 0, 1))
return self.tcp_to_ee_link_pose_target(pose, ee_link).position
def tcp_to_ee_link_quaternion_target(self, quaternion, ee_link):
pose = Pose(Point(0, 0, 0), quaternion)
return self.tcp_to_ee_link_pose_target(pose, ee_link).orientation
def tcp_to_ee_link_rpy_target(self, roll, pitch, yaw, ee_link):
qx, qy, qz, qw = quaternion_from_euler(roll, pitch, yaw)
pose = Pose(Point(0, 0, 0), Quaternion(qx, qy, qz, qw))
pose = self.tcp_to_ee_link_pose_target(pose, ee_link)
new_roll, new_pitch, new_yaw = euler_from_quaternion(*pose.orientation)
return new_roll, new_pitch, new_yaw
def set_empty_tcp_to_ee_link_transform(self, ee_link):
ee_link_to_tcp_transform = self.transform_from_pose(
Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)), ee_link, "TCP")
self.__static_broadcaster.sendTransform(ee_link_to_tcp_transform)
return ee_link_to_tcp_transform
def lookup_transform(self, target_frame, source_frame, stamp=None):
if stamp:
return self.__tf_buffer.lookup_transform(target_frame,
source_frame, stamp)
return self.__tf_buffer.lookup_transform(target_frame, source_frame,
rospy.Time(0))
def display_target_pose(self, pose, name="target_pose"):
t = self.transform_from_pose(pose, "base_link", name)
self.__static_broadcaster.sendTransform(t)
@staticmethod
def transform_from_pose(pose, parent_frame, child_frame, stamp=None):
t = TransformStamped()
t.transform.translation.x = pose.position.x
t.transform.translation.y = pose.position.y
t.transform.translation.z = pose.position.z
t.transform.rotation = pose.orientation
t.header.frame_id = parent_frame
t.child_frame_id = child_frame
if stamp:
t.header.stamp = stamp
return t
@staticmethod
def pose_from_transform(transform):
pose = Pose()
pose.position.x = transform.translation.x
pose.position.y = transform.translation.y
pose.position.z = transform.translation.z
pose.orientation = transform.rotation
return pose
class EPuckDriver(WebotsDifferentialDriveNode):
def __init__(self, args):
super().__init__('epuck_driver',
args,
wheel_distance=DEFAULT_WHEEL_DISTANCE,
wheel_radius=DEFAULT_WHEEL_RADIUS)
self.start_device_manager(DEVICE_CONFIG)
# Intialize distance sensors for LaserScan topic
self.distance_sensors = {}
for i in range(NB_INFRARED_SENSORS):
sensor = self.robot.getDistanceSensor('ps{}'.format(i))
sensor.enable(self.timestep)
self.distance_sensors['ps{}'.format(i)] = sensor
self.laser_publisher = self.create_publisher(LaserScan, '/scan', 1)
self.tof_sensor = self.robot.getDistanceSensor('tof')
if self.tof_sensor:
self.tof_sensor.enable(self.timestep)
else:
self.get_logger().info(
'ToF sensor is not present for this e-puck version')
laser_transform = TransformStamped()
laser_transform.header.stamp = Time(
seconds=self.robot.getTime()).to_msg()
laser_transform.header.frame_id = 'base_link'
laser_transform.child_frame_id = 'laser_scanner'
laser_transform.transform.rotation.x = 0.0
laser_transform.transform.rotation.y = 0.0
laser_transform.transform.rotation.z = 0.0
laser_transform.transform.rotation.w = 1.0
laser_transform.transform.translation.x = 0.0
laser_transform.transform.translation.y = 0.0
laser_transform.transform.translation.z = 0.033
self.static_broadcaster = StaticTransformBroadcaster(self)
self.static_broadcaster.sendTransform(laser_transform)
# Main loop
self.create_timer(self.timestep / 1000, self.__publish_laserscan_data)
def __publish_laserscan_data(self):
stamp = Time(seconds=self.robot.getTime()).to_msg()
dists = [OUT_OF_RANGE] * NB_INFRARED_SENSORS
dist_tof = OUT_OF_RANGE
# Calculate distances
for i, key in enumerate(self.distance_sensors):
dists[i] = interpolate_lookup_table(
self.distance_sensors[key].getValue(),
self.distance_sensors[key].getLookupTable())
# Publish range: ToF
if self.tof_sensor:
dist_tof = interpolate_lookup_table(
self.tof_sensor.getValue(), self.tof_sensor.getLookupTable())
# Max range of ToF sensor is 2m so we put it as maximum laser range.
# Therefore, for all invalid ranges we put 0 so it get deleted by rviz
laser_dists = [
OUT_OF_RANGE if dist > INFRARED_MAX_RANGE else dist
for dist in dists
]
msg = LaserScan()
msg.header.frame_id = 'laser_scanner'
msg.header.stamp = stamp
msg.angle_min = -150 * pi / 180
msg.angle_max = 150 * pi / 180
msg.angle_increment = 15 * pi / 180
msg.range_min = SENSOR_DIST_FROM_CENTER + INFRARED_MIN_RANGE
msg.range_max = SENSOR_DIST_FROM_CENTER + TOF_MAX_RANGE
msg.ranges = [
laser_dists[3] + SENSOR_DIST_FROM_CENTER, # -150
OUT_OF_RANGE, # -135
OUT_OF_RANGE, # -120
OUT_OF_RANGE, # -105
laser_dists[2] + SENSOR_DIST_FROM_CENTER, # -90
OUT_OF_RANGE, # -75
OUT_OF_RANGE, # -60
laser_dists[1] + SENSOR_DIST_FROM_CENTER, # -45
OUT_OF_RANGE, # -30
laser_dists[0] + SENSOR_DIST_FROM_CENTER, # -15
dist_tof + SENSOR_DIST_FROM_CENTER, # 0
laser_dists[7] + SENSOR_DIST_FROM_CENTER, # 15
OUT_OF_RANGE, # 30
laser_dists[6] + SENSOR_DIST_FROM_CENTER, # 45
OUT_OF_RANGE, # 60
OUT_OF_RANGE, # 75
laser_dists[5] + SENSOR_DIST_FROM_CENTER, # 90
OUT_OF_RANGE, # 105
OUT_OF_RANGE, # 120
OUT_OF_RANGE, # 135
laser_dists[4] + SENSOR_DIST_FROM_CENTER, # 150
]
self.laser_publisher.publish(msg)
class LidarDevice(SensorDevice):
"""
ROS2 wrapper for Webots Lidar node.
Creates suitable ROS2 interface based on Webots [Lidar](https://cyberbotics.com/doc/reference/lidar) node instance:
It allows the following functinalities:
- Publishes range measurements of type `sensor_msgs/LaserScan` if 2D Lidar is present
- Publishes range measurements of type `sensor_msgs/PointCloud2` if 3D Lidar is present
Args:
node (WebotsNode): The ROS2 node.
device_key (str): Unique identifier of the device used for configuration.
wb_device (Lidar): Webots node of type Lidar.
Kwargs:
params (dict): Inherited from `SensorDevice` + the following::
dict: {
'noise': int, # Maximum sensor noise used to compensate the maximum range (default 0.01)
'timestep': int, # Publish period in ms (default 128ms)
}
"""
def __init__(self, node, device_key, wb_device, params=None):
super().__init__(node, device_key, wb_device, params)
self.__publishers = {}
self.__static_transforms = []
self.__static_broadcaster = None
self.__noise = self._get_param('noise', 1e-2)
# Exit if disabled
if self._disable:
return
# Change default timestep
self._timestep = 128
qos_sensor_reliable = qos_profile_sensor_data
qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE
# Create topics
if wb_device.getNumberOfLayers() > 1:
wb_device.enablePointCloud()
self.__publisher = node.create_publisher(PointCloud2,
self._topic_name,
qos_sensor_reliable)
else:
self.__publisher = node.create_publisher(LaserScan,
self._topic_name,
qos_sensor_reliable)
self.__static_broadcaster = StaticTransformBroadcaster(node)
transform_stamped = TransformStamped()
transform_stamped.header.frame_id = self._frame_id
transform_stamped.child_frame_id = self._frame_id + '_rotated'
transform_stamped.transform.rotation.x = 0.5
transform_stamped.transform.rotation.y = 0.5
transform_stamped.transform.rotation.z = -0.5
transform_stamped.transform.rotation.w = 0.5
self.__static_broadcaster.sendTransform(transform_stamped)
def step(self):
stamp = super().step()
if not stamp:
return
if self.__publisher.get_subscription_count(
) > 0 or self._always_publish:
self._wb_device.enable(self._timestep)
if self._wb_device.getNumberOfLayers() > 1:
self.__publish_point_cloud_data(stamp)
else:
self.__publish_laser_scan_data(stamp)
else:
self._wb_device.disable()
def __publish_point_cloud_data(self, stamp):
data = self._wb_device.getPointCloud(data_type='buffer')
if data:
msg = PointCloud2()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.height = 1
msg.width = self._wb_device.getNumberOfPoints()
msg.point_step = 20
msg.row_step = 20 * self._wb_device.getNumberOfPoints()
msg.is_dense = False
msg.fields = [
PointField(name='x',
offset=0,
datatype=PointField.FLOAT32,
count=1),
PointField(name='y',
offset=4,
datatype=PointField.FLOAT32,
count=1),
PointField(name='z',
offset=8,
datatype=PointField.FLOAT32,
count=1)
]
msg.is_bigendian = False
# We pass `data` directly to we avoid using `data` setter.
# Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally.
# Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable
# behavior.
# deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`.
msg._data = data
self.__publisher.publish(msg)
def __publish_laser_scan_data(self, stamp):
"""Publish the laser scan topics with up to date value."""
ranges = self._wb_device.getLayerRangeImage(0)
if ranges:
msg = LaserScan()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id + '_rotated'
msg.angle_min = -0.5 * self._wb_device.getFov()
msg.angle_max = 0.5 * self._wb_device.getFov()
msg.angle_increment = self._wb_device.getFov() / (
self._wb_device.getHorizontalResolution() - 1)
msg.scan_time = self._wb_device.getSamplingPeriod() / 1000.0
msg.range_min = self._wb_device.getMinRange() + self.__noise
msg.range_max = self._wb_device.getMaxRange() - self.__noise
msg.ranges = ranges
self.__publisher.publish(msg)
def __init__(self):
self.__tf_buffer = Buffer()
self.__tf_listener = TransformListener(self.__tf_buffer)
self.__static_broadcaster = StaticTransformBroadcaster()
th_w = quat[3]
self.static_transformStamped.header.stamp = rospy.Time.now()
self.static_transformStamped.header.frame_id = parent
self.static_transformStamped.child_frame_id = child
self.static_transformStamped.transform.translation.x = x
self.static_transformStamped.transform.translation.y = y
self.static_transformStamped.transform.translation.z = z
self.static_transformStamped.transform.rotation.x = th_x
self.static_transformStamped.transform.rotation.y = th_y
self.static_transformStamped.transform.rotation.z = th_z
self.static_transformStamped.transform.rotation.w = th_w
if __name__ == '__main__':
rospy.init_node('static_tf_node')
print "Node 'static_tf_node' has initialized."
broadcaster = StaticTransformBroadcaster()
base_link = createTf("base_footprint", "base_link", 0, 0, 0.05415, 0, 0, 0)
base_scan = createTf("base_link", "base_scan", 0, 0, 0.23085, 0, 0, 0)
wheel_left_link = createTf("base_link", "wheel_left_link", -0.09, 0.152, 0, -1.5708, 0, 0)
wheel_right_link = createTf("base_link", "wheel_right_link", -0.09, -0.152, 0, -1.5708, 0, 0)
imu_link = createTf("base_link", "imu_link", 0.183, 0.051882, 0.14285, 0, 0, 0)
broadcaster.sendTransform([base_link.static_transformStamped])
rospy.spin()
class InterfaceForRobot():
def __init__(self, cowork=False):
self.stl_pkg_path = r.get_path("object_description")
self.grasping_pose = grasp_poses.grasping_pose
self.br = StaticTransformBroadcaster()
if cowork:
moveit_commander.roscpp_initialize(sys.argv)
self.scene = moveit_commander.PlanningSceneInterface()
self.client_for_robot = rospy.ServiceProxy("/to_HoleCheck",
asm_Srv)
self.client_asm_check = rospy.ServiceProxy("/to_RobotControl",
asm_Srv)
rospy.wait_for_service("/to_HoleCheck")
rospy.wait_for_service("/to_RobotControl")
else:
pass
def req_msg_for_HoleCheck(self, assembly_type, ref_obj_name,
ref_const_names, move_obj_name,
move_const_names):
parent = Asm_test([ref_obj_name], ref_const_names)
child = Asm_test([move_obj_name], move_const_names)
print("\n--- HoleCheck")
print(assembly_type, parent, child)
resp = self.client_for_robot(assembly_type, parent, child)
print(resp)
return resp
def req_msg_for_RobotControl(self, assembly_type, ref_obj_name,
ref_const_names, move_obj_name,
move_const_names):
parent = Asm_test([ref_obj_name], ref_const_names)
child = Asm_test([move_obj_name], move_const_names)
print("\n--- RobotControl")
print(assembly_type, parent, child)
resp = self.client_asm_check(assembly_type, parent, child)
print(resp)
return resp
def publish_obj_tf(self, obj, obj_pose):
target_posestamped = PoseStamped()
target_posestamped.header.stamp = rospy.Time.now()
target_posestamped.header.frame_id = "table"
target_posestamped.pose = obj_pose
target_name = obj.referencePart
target_type = target_name.split("_")[0]
stl_path = os.path.join(self.stl_pkg_path, "urdfs", target_type+".SLDPRT", \
"meshes", target_type+".SLDPRT.STL")
self.scene.add_mesh(target_name, target_posestamped, stl_path)
self.send_tf_by_pose(obj_pose, target_name, "table")
self.send_const_tf(obj, obj_pose)
self.send_grasp_tf(obj)
def publish_just_tf(self, obj_name, obj_pose, reference):
target_posestamped = PoseStamped()
target_posestamped.header.stamp = rospy.Time.now()
target_posestamped.header.frame_id = reference
target_posestamped.pose = obj_pose
target_name = obj_name
target_type = target_name.split("_")[0]
stl_path = os.path.join(self.stl_pkg_path, "urdfs", target_type+".SLDPRT", \
"meshes", target_type+".SLDPRT.STL")
self.scene.add_mesh(target_name, target_posestamped, stl_path)
self.send_tf_by_pose(obj_pose, target_name, reference)
def send_tf_by_pose(self, pose, target_name, ref_name="world"):
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = ref_name
t.child_frame_id = target_name
t.transform.translation.x = pose.position.x
t.transform.translation.y = pose.position.y
t.transform.translation.z = pose.position.z
t.transform.rotation.x = pose.orientation.x
t.transform.rotation.y = pose.orientation.y
t.transform.rotation.z = pose.orientation.z
t.transform.rotation.w = pose.orientation.w
self.br.sendTransform(t)
def send_const_tf(self, obj, obj_pose):
target_consts = obj.assemConsts
for const_name, const in target_consts.items():
const_end_coor = const["endCoordinate"]
const_end_pose = self.get_pose_from_tf_mat(const_end_coor).pose
self.send_tf_by_pose(const_end_pose, const_name + "_end",
const["parent"])
const_entry_coor = const["entryCoordinate"]
const_entry_pose = self.get_pose_from_tf_mat(const_entry_coor).pose
self.send_tf_by_pose(const_entry_pose, const_name + "_entry",
const["parent"])
def send_grasp_tf(self, obj):
target_obj_type = obj.obj_type
target_obj_name = obj.referencePart
if target_obj_type in self.grasping_pose.keys():
target_grasping_pose = self.grasping_pose[target_obj_type]
idx = 1
for grasping_pose_dict in target_grasping_pose:
grasp_tr = grasping_pose_dict["tr"]
grasp_rot = grasping_pose_dict["rot"]
grasp_pose = self.get_pose_from_tr_rot(grasp_tr,
grasp_rot).pose
pose_name = "{}-{}-{}".format(target_obj_name, "GRASP", idx)
#print(pose_name)
self.send_tf_by_pose(grasp_pose, pose_name, target_obj_name)
idx += 1
else:
pass
def get_pose_from_tr_quat(self, tr, quat):
posestamped = PoseStamped()
posestamped.pose.position.x = tr[0]
posestamped.pose.position.y = tr[1]
posestamped.pose.position.z = tr[2]
posestamped.pose.orientation.x = quat[0]
posestamped.pose.orientation.y = quat[1]
posestamped.pose.orientation.z = quat[2]
posestamped.pose.orientation.w = quat[3]
return posestamped
def get_pose_from_tr_rot(self, tr, rot):
quat = tf.quaternion_from_euler(rot[0], rot[1], rot[2])
posestamped = PoseStamped()
posestamped.pose.position.x = tr[0]
posestamped.pose.position.y = tr[1]
posestamped.pose.position.z = tr[2]
posestamped.pose.orientation.x = quat[0]
posestamped.pose.orientation.y = quat[1]
posestamped.pose.orientation.z = quat[2]
posestamped.pose.orientation.w = quat[3]
return posestamped
def get_pose_from_tf_mat(self, tf_mat):
quat = tf.quaternion_from_matrix(tf_mat)
tr = tf.translation_from_matrix(tf_mat)
posestamped = self.get_pose_from_tr_quat(tr, quat)
return posestamped
#!/usr/bin/env python
import rospy
from tf2_ros import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
from geographic_msgs.msg import GeoPoint
from geodesy import utm
if __name__=="__main__":
rospy.init_node("utm_to_world")
datum = map(float, rospy.get_param("~datum", []).strip('][').split(','))
datum = GeoPoint(*map(float, datum))
utmpoint = utm.fromMsg(datum)
broadcaster = StaticTransformBroadcaster()
tf = TransformStamped()
tf.header.stamp = rospy.Time.now()
tf.header.frame_id = "utm"
tf.child_frame_id = "world"
tf.transform.translation.x = utmpoint.easting
tf.transform.translation.y = utmpoint.northing
tf.transform.translation.z = utmpoint.altitude
tf.transform.rotation.w = 1.0
broadcaster.sendTransform(tf)
rospy.spin()
class Tf2Broadcaster(Node):
def __init__(self,
parent_frame_id: str = "world",
child_frame_id: str = "unknown_child_id",
use_sim_time: bool = True,
node_name: str = 'drl_grasping_camera_tf_broadcaster'):
try:
rclpy.init()
except:
if not rclpy.ok():
import sys
sys.exit("ROS 2 could not be initialised")
Node.__init__(self, node_name)
self.set_parameters([
Parameter('use_sim_time',
type_=Parameter.Type.BOOL,
value=use_sim_time)
])
qos = QoSProfile(durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_ALL)
self._tf2_broadcaster = StaticTransformBroadcaster(self, qos=qos)
self._transform_stamped = TransformStamped()
self.set_parent_frame_id(parent_frame_id)
self.set_child_frame_id(child_frame_id)
def set_parent_frame_id(self, parent_frame_id: str):
self._transform_stamped.header.frame_id = parent_frame_id
def set_child_frame_id(self, child_frame_id: str):
self._transform_stamped.child_frame_id = child_frame_id
def broadcast_tf(self,
translation: Tuple[float, float, float],
rotation: Tuple[float, float, float, float],
xyzw: bool = True,
parent_frame_id: str = None,
child_frame_id: str = None):
"""
Broadcast transformation of the camera
"""
transform = self._transform_stamped
if parent_frame_id is not None:
transform.header.frame_id = parent_frame_id
if child_frame_id is not None:
transform.child_frame_id = child_frame_id
transform.header.stamp = self.get_clock().now().to_msg()
transform.transform.translation.x = float(translation[0])
transform.transform.translation.y = float(translation[1])
transform.transform.translation.z = float(translation[2])
if xyzw:
transform.transform.rotation.x = float(rotation[0])
transform.transform.rotation.y = float(rotation[1])
transform.transform.rotation.z = float(rotation[2])
transform.transform.rotation.w = float(rotation[3])
else:
transform.transform.rotation.w = float(rotation[0])
transform.transform.rotation.x = float(rotation[1])
transform.transform.rotation.y = float(rotation[2])
transform.transform.rotation.z = float(rotation[3])
self._tf2_broadcaster.sendTransform(transform)
master_gps = rospy.Publisher('gps_datum', NavSatFix, queue_size=5)
master_msg = NavSatFix()
master_msg.altitude = alt
master_msg.longitude = lon
master_msg.latitude = lat
master_msg.status = NavSatStatus()
master_msg.status.status = master_msg.status.STATUS_FIX
master_msg.status.service = master_msg.status.SERVICE_GPS
master_msg.position_covariance_type = master_msg.COVARIANCE_TYPE_UNKNOWN
master_msg.position_covariance = [-1, 0, 0, 0, 0, 0, 0, 0, 0]
pose_pub = rospy.Publisher('waterlinked/pose_with_cov_stamped',
PoseWithCovarianceStamped,
queue_size=5)
buff = Buffer()
TransformListener(buff)
rospy.Subscriber("mavros/global_position/global", NavSatFix, handle_gps,
(pose_pub, buff))
map_to_waterlink = TransformStamped(
Header(0, rospy.Time.now(), 'map'), 'waterlinked',
Transform(
None,
Quaternion(*Rotation.from_euler('xyz', [0, 0, 90 - heading_offset],
degrees=True).as_quat().tolist())))
StaticTransformBroadcaster().sendTransform(map_to_waterlink)
rate = rospy.Rate(4.0)
while not rospy.is_shutdown():
master_msg.header.seq += 1
master_msg.header.stamp = rospy.Time.now()
master_gps.publish(master_msg)
rate.sleep()
