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 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 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 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()
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)
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)
def updateImage(self, img):
print("Looking for objects")
arr = self.bridge.imgmsg_to_cv2(img, "bgr8")
font = cv2.FONT_HERSHEY_COMPLEX
#shape of object in 3d space
sign_width = 0.2
sign_height = 0.2
#corners in 3d space
corners_object3d = np.array([[0, 0, 0], [sign_width, 0, 0],
[sign_width, sign_height, 0],
[0, sign_height, 0]])
#camera params
mtx = np.array([[231.250001, 0.000000, 320.519378],
[0.000000, 231.065552, 240.631482],
[0.000000, 0.000000, 1.000000]])
dist = np.array([0.061687, -0.049761, -0.008166, 0.004284, 0.000000])
if self.image_lock.acquire(True):
self.img = arr
if self.model is None:
self.model = self.get_model()
self.img_out, self.boxes = self.predict(self.model, self.img)
self.img_out = np.asarray(self.img_out[0, :, :, :])
pad = 7
canny = cv2.Canny(self.img_out, 100, 200, 3)
#480 * 640 image dimension
for box in self.boxes:
#bgr
#draw boxes
cv2.rectangle(
self.img_out, (box['topleft']['x'], box['topleft']['y']),
(box['bottomright']['x'], box['bottomright']['y']),
(207, 161, 146), 3)
#draw label
cv2.putText(self.img_out, box['label'],
(box['topleft']['x'], box['topleft']['y'] - 12), 0,
0.6, (255, 0, 0), 6 // 3)
#DEPTH###############################################################################################################33
#clockwise from top left top right bottom right and finally bottom left
c1 = np.array([box['topleft']['x'], box['topleft']['y']],
dtype='f')
c2 = np.array([box['bottomright']['x'], box['topleft']['y']],
dtype='f')
c3 = np.array(
[box['bottomright']['x'], box['bottomright']['y']],
dtype='f')
c4 = np.array([box['topleft']['x'], box['bottomright']['y']],
dtype='f')
#corneres in image plane
corners = np.array([[c1], [c2], [c3], [c4]])
#Estimate pose of each marker and return the values rvet and tvec---different from camera coefficients
#translation and rotation btw camera and world co-ordinates
_, rvecs, tvecs = cv2.solvePnP(corners_object3d, corners, mtx,
dist)
rotMatrix, _ = cv2.Rodrigues(rvecs)
#print("rot ",rotMatrix.shape)
#print("trans ",tvecs.shape)
convMatrix = np.hstack((rotMatrix, tvecs))
#print("conv ",convMatrix.shape)
#center in 3d space
#center3d = np.zeros((4,1))
center3d = np.asarray([[sign_width / 2], [sign_height / 2],
[0], [1]])
#print("center 3d ", center3d.shape)
#final object in world w.r.t camera frame
centerWorld = np.matmul(convMatrix, center3d)
#print("center tranformed ", centerWorld.shape)
print("wrt camera", np.ravel(centerWorld))
#center in camera frame
centerCamera = PoseStamped()
centerCamera.header.stamp = rospy.Time.now()
centerCamera.header.frame_id = "camera_link"
centerCamera.pose.position.x = centerWorld[0]
centerCamera.pose.position.y = centerWorld[1]
centerCamera.pose.position.z = centerWorld[2]
# quaternion = tf.transformations.quaternion_from_euler(0, 0, self.theta)
# centerPose.pose.orientation.x = quaternion[0]
# centerPose.pose.orientation.y = quaternion[1]
# centerPose.pose.orientation.z = quaternion[2]
# centerPose.pose.orientation.w = quaternion[3]
if not self.tf_buf.can_transform(centerCamera.header.frame_id,
'map',
centerCamera.header.stamp):
rospy.logwarn_throttle(
5.0, 'No transform from %s to map' %
centerCamera.header.frame_id)
return
#tranforming center in camera frame ro map frame
centerMap = self.tf_buf.transform(centerCamera, 'map')
print("in map", np.ravel(centerWorld))
detection = np.hstack((centerWorld, box['label']))
print("in map", np.ravel(detection))
print(detection.shape)
self.allDetections.append(centerMap)
trans = TransformStamped()
trans.header.frame_id = 'map'
trans.child_frame_id = box['label']
trans.transform.translation.x = centerMap.pose.position.x
trans.transform.translation.y = centerMap.pose.position.y
trans.transform.translation.z = centerMap.pose.position.z
trans.transform.rotation.x = centerMap.pose.orientation.x
trans.transform.rotation.y = centerMap.pose.orientation.y
trans.transform.rotation.z = centerMap.pose.orientation.z
trans.transform.rotation.w = centerMap.pose.orientation.W
self.L.append(trans)
stb = StaticTransformBroadcaster()
stb.sendTransform(self.L)
for item in self.L:
self.poseFile.write("%s\n" % item)
#DEPTH###############################################################################################################33
self.image_lock.release()
class Localisation(rclpy.node.Node):
"""Robot localisation node"""
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 _on_set_parameters(self, params):
"""Handle Parameter events especially for side"""
result = SetParametersResult()
try:
for param in params:
if param.name == "side":
self.get_logger().warn(f"Side changed {param.value}")
self.side = param
else:
setattr(self, param.name, param)
(
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.update_transform()
result.successful = True
except BaseException as e:
result.reason = e
return result
def fetchStartPosition(self):
"""Fetch start position for side and robot"""
if self.robot == "asterix":
if self.side.value == "blue":
return (0.29, 1.33, 0)
elif self.side.value == "yellow":
return (0.29, 1.33, 0)
elif self.robot == "obelix":
if self.side.value == "blue":
return (0.29, 1.33, 0)
elif self.side.value == "yellow":
return (3 - 0.29, 1.33, np.pi)
return None
def update_transform(self):
"""Update and publish odom --> map transform"""
self._tf.header.stamp = self.get_clock().now().to_msg()
self._tf.transform.translation.x = self.robot_pose.pose.position.x
self._tf.transform.translation.y = self.robot_pose.pose.position.y
self._tf.transform.rotation = self.robot_pose.pose.orientation
self._initial_tf = copy.deepcopy(self._tf)
self._tf_brodcaster.sendTransform(self._tf)
def allies_subscription_callback(self, msg):
"""Identity the robot marker in assurancetourix marker_array detection,
send the marker to vlx_readjustment in order to try to refine the position
at a stamp given by the marker"""
for ally_marker in msg.markers:
if (
ally_marker.text.lower() == self.robot
and (self.get_clock().now().to_msg().sec - self.last_odom_update) > 0.4
):
if (
is_simulation()
): # simulate marker delais (image analysis from assurancetourix)
time.sleep(0.15)
if not (
ally_marker.pose.position.x < 0.2
or ally_marker.pose.position.x > 2.8
or ally_marker.pose.position.y < 0.2
or ally_marker.pose.position.y > 1.8
):
if self.vlx.continuous_sampling == 0:
self.create_and_send_tf(
ally_marker.pose.position.x,
ally_marker.pose.position.y,
ally_marker.pose.orientation,
ally_marker.header.stamp,
)
else:
self.vlx.try_to_readjust_with_vlx(
ally_marker.pose.position.x,
ally_marker.pose.position.y,
ally_marker.pose.orientation,
ally_marker.header.stamp,
)
if self.vlx.continuous_sampling in [0, 1]:
self.vlx.start_continuous_sampling_thread(0.65, 1)
def is_near_walls(self, pt):
"""Return true if the robot if less than 30cm from the wall"""
return pt.x < 0.3 or pt.y < 0.3 or pt.x > 2.7 or pt.y > 1.7
def create_and_send_tf(self, x, y, q, stamp):
"""Create and send tf to drive for it to re-adjust odometry"""
self._tf.header.stamp = stamp
self._tf.transform.translation.x = x
self._tf.transform.translation.y = y
self._tf.transform.translation.z = float(0)
self._tf.transform.rotation = q
self.last_odom_update = self.get_clock().now().to_msg().sec
self.tf_publisher_.publish(self._tf)
def odom_callback(self, msg):
"""Odom callback, computes the new pose of the robot relative to map,
send this new pose on odom_map_relative topic and start vlx routine
if this pose is near walls"""
robot_tf = TransformStamped()
robot_tf.transform.translation.x = msg.pose.pose.position.x
robot_tf.transform.translation.y = msg.pose.pose.position.y
robot_tf.transform.rotation = msg.pose.pose.orientation
robot_frame = transform_to_kdl(robot_tf)
new_robot_pose_kdl = do_transform_frame(robot_frame, self._initial_tf)
self.robot_pose.pose.position.x = new_robot_pose_kdl.p.x()
self.robot_pose.pose.position.y = new_robot_pose_kdl.p.y()
q = new_robot_pose_kdl.M.GetQuaternion()
self.robot_pose.header.stamp = msg.header.stamp
self.robot_pose.header.frame_id = "map"
self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
self.odom_map_relative_publisher_.publish(self.robot_pose)
if self.is_near_walls(self.robot_pose.pose.position):
if self.vlx.continuous_sampling == 2:
self.vlx.near_wall_routine(self.robot_pose)
else:
self.vlx.start_near_wall_routine(self.robot_pose)
elif self.vlx.continuous_sampling == 2:
self.vlx.stop_near_wall_routine()
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
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 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
def __init__(self, args):
super().__init__('epuck_driver', args)
# Parameters
wheel_distance_param = self.declare_parameter("wheel_distance", 0.0552)
wheel_radius_param = self.declare_parameter("wheel_radius", 0.021)
camera_period_param = self.declare_parameter(
"camera_period", self.timestep)
self.period = self.declare_parameter("period", self.timestep)
self.camera_period = camera_period_param.value
self.wheel_radius = wheel_radius_param.value
self.wheel_distance = wheel_distance_param.value
self.set_parameters_callback(self.on_param_changed)
# Initialize motors
self.left_motor = self.robot.getMotor('left wheel motor')
self.right_motor = self.robot.getMotor('right wheel motor')
self.left_motor.setPosition(float('inf'))
self.right_motor.setPosition(float('inf'))
self.left_motor.setVelocity(0)
self.right_motor.setVelocity(0)
self.create_subscription(Twist, '/cmd_vel', self.cmd_vel_callback, 1)
self.get_logger().info('EPuck Initialized')
# Initialize odometry
self.reset_odometry()
self.left_wheel_sensor = self.robot.getPositionSensor(
'left wheel sensor')
self.right_wheel_sensor = self.robot.getPositionSensor(
'right wheel sensor')
self.left_wheel_sensor.enable(self.period.value)
self.right_wheel_sensor.enable(self.period.value)
self.odometry_publisher = self.create_publisher(Odometry, '/odom', 1)
# Initialize IMU
self.gyro = self.robot.getGyro('gyro')
if self.gyro:
self.gyro.enable(self.period.value)
else:
self.get_logger().info('Gyroscope is not present for this e-puck version')
self.accelerometer = self.robot.getAccelerometer('accelerometer')
self.accelerometer.enable(self.period.value)
self.imu_publisher = self.create_publisher(Imu, '/imu', 10)
# Initialize ground sensors
self.ground_sensors = {}
self.ground_sensor_publishers = {}
self.ground_sensor_broadcasters = []
for i in range(3):
idx = 'gs{}'.format(i)
ground_sensor = self.robot.getDistanceSensor(idx)
if ground_sensor:
ground_sensor.enable(self.period.value)
self.ground_sensors[idx] = ground_sensor
self.ground_sensor_publishers[idx] = self.create_publisher(
Range, '/' + idx, 1)
ground_sensor_broadcaster = StaticTransformBroadcaster(self)
ground_sensor_transform = TransformStamped()
ground_sensor_transform.header.stamp = self.now()
ground_sensor_transform.header.frame_id = "base_link"
ground_sensor_transform.child_frame_id = "gs" + str(i)
ground_sensor_transform.transform.rotation = euler_to_quaternion(
0, pi/2, 0)
ground_sensor_transform.transform.translation.x = SENSOR_DIST_FROM_CENTER - 0.005
ground_sensor_transform.transform.translation.y = 0.009 - i * 0.009
ground_sensor_transform.transform.translation.z = 0.0
ground_sensor_broadcaster.sendTransform(
ground_sensor_transform)
self.ground_sensor_broadcasters.append(
ground_sensor_broadcaster)
else:
self.get_logger().info(
'Ground sensor `{}` is not present for this e-puck version'.format(idx))
# Intialize distance sensors
self.distance_sensor_publishers = {}
self.distance_sensors = {}
self.distance_sensor_broadcasters = []
for i in range(8):
sensor = self.robot.getDistanceSensor('ps{}'.format(i))
sensor.enable(self.period.value)
sensor_publisher = self.create_publisher(
Range, '/ps{}'.format(i), 10)
self.distance_sensors['ps{}'.format(i)] = sensor
self.distance_sensor_publishers['ps{}'.format(
i)] = sensor_publisher
distance_sensor_broadcaster = StaticTransformBroadcaster(self)
distance_sensor_transform = TransformStamped()
distance_sensor_transform.header.stamp = self.now()
distance_sensor_transform.header.frame_id = "base_link"
distance_sensor_transform.child_frame_id = "ps" + str(i)
distance_sensor_transform.transform.rotation = euler_to_quaternion(
0, 0, DISTANCE_SENSOR_ANGLE[i])
distance_sensor_transform.transform.translation.x = SENSOR_DIST_FROM_CENTER * \
cos(DISTANCE_SENSOR_ANGLE[i])
distance_sensor_transform.transform.translation.y = SENSOR_DIST_FROM_CENTER * \
sin(DISTANCE_SENSOR_ANGLE[i])
distance_sensor_transform.transform.translation.z = 0.0
distance_sensor_broadcaster.sendTransform(
distance_sensor_transform)
self.distance_sensor_broadcasters.append(
distance_sensor_broadcaster)
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.period.value)
self.tof_publisher = self.create_publisher(Range, '/tof', 1)
self.tof_broadcaster = StaticTransformBroadcaster(self)
tof_transform = TransformStamped()
tof_transform.header.stamp = self.now()
tof_transform.header.frame_id = "base_link"
tof_transform.child_frame_id = "tof"
tof_transform.transform.rotation.x = 0.0
tof_transform.transform.rotation.y = 0.0
tof_transform.transform.rotation.z = 0.0
tof_transform.transform.rotation.w = 1.0
tof_transform.transform.translation.x = SENSOR_DIST_FROM_CENTER
tof_transform.transform.translation.y = 0.0
tof_transform.transform.translation.z = 0.0
self.tof_broadcaster.sendTransform(tof_transform)
else:
self.get_logger().info('ToF sensor is not present for this e-puck version')
# Initialize camera
self.camera = self.robot.getCamera('camera')
self.camera.enable(self.camera_period)
self.camera_publisher = self.create_publisher(
Image, '/image_raw', 10)
self.create_timer(self.camera_period / 1000, self.camera_callback)
self.camera_info_publisher = self.create_publisher(
CameraInfo, '/camera_info', 10)
# Initialize binary LEDs
self.binary_leds = []
self.binary_led_subscribers = []
for i in range(NB_BINARY_LEDS):
index = i * 2
led = self.robot.getLED('led{}'.format(index))
led_subscriber = self.create_subscription(
Bool,
'/led{}'.format(index),
partial(self.on_binary_led_callback, index=i),
10
)
self.binary_leds.append(led)
self.binary_led_subscribers.append(led_subscriber)
# Initialize RGB LEDs
self.rgb_leds = []
self.rgb_led_subscribers = []
for i in range(NB_RGB_LEDS):
index = i * 2 + 1
led = self.robot.getLED('led{}'.format(index))
led_subscriber = self.create_subscription(
Int32,
'/led{}'.format(index),
partial(self.on_rgb_led_callback, index=i),
10
)
self.rgb_leds.append(led)
self.rgb_led_subscribers.append(led_subscriber)
# Initialize Light sensors
self.light_sensors = []
self.light_publishers = []
self.light_sensor_broadcasters = []
for i in range(NB_LIGHT_SENSORS):
light_sensor = self.robot.getLightSensor(f'ls{i}')
light_sensor.enable(self.period.value)
light_publisher = self.create_publisher(Illuminance, f'/ls{i}', 1)
self.light_publishers.append(light_publisher)
self.light_sensors.append(light_sensor)
light_sensor_broadcaster = StaticTransformBroadcaster(self)
light_transform = TransformStamped()
light_transform.header.stamp = self.now()
light_transform.header.frame_id = "base_link"
light_transform.child_frame_id = "ls" + str(i)
light_transform.transform.rotation = euler_to_quaternion(
0, 0, DISTANCE_SENSOR_ANGLE[i])
light_transform.transform.translation.x = SENSOR_DIST_FROM_CENTER * \
cos(DISTANCE_SENSOR_ANGLE[i])
light_transform.transform.translation.y = SENSOR_DIST_FROM_CENTER * \
sin(DISTANCE_SENSOR_ANGLE[i])
light_transform.transform.translation.z = 0.0
light_sensor_broadcaster.sendTransform(light_transform)
self.light_sensor_broadcasters.append(light_sensor_broadcaster)
# Static tf broadcaster: Laser
self.laser_broadcaster = StaticTransformBroadcaster(self)
laser_transform = TransformStamped()
laser_transform.header.stamp = self.now()
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.0
self.laser_broadcaster.sendTransform(laser_transform)
# Main loop
self.create_timer(self.period.value / 1000, self.step_callback)
# Transforms
self.tf_broadcaster = TransformBroadcaster(self)
class EPuckDriver(WebotsNode):
def __init__(self, args):
super().__init__('epuck_driver', args)
# Parameters
wheel_distance_param = self.declare_parameter("wheel_distance", 0.0552)
wheel_radius_param = self.declare_parameter("wheel_radius", 0.021)
camera_period_param = self.declare_parameter(
"camera_period", self.timestep)
self.period = self.declare_parameter("period", self.timestep)
self.camera_period = camera_period_param.value
self.wheel_radius = wheel_radius_param.value
self.wheel_distance = wheel_distance_param.value
self.set_parameters_callback(self.on_param_changed)
# Initialize motors
self.left_motor = self.robot.getMotor('left wheel motor')
self.right_motor = self.robot.getMotor('right wheel motor')
self.left_motor.setPosition(float('inf'))
self.right_motor.setPosition(float('inf'))
self.left_motor.setVelocity(0)
self.right_motor.setVelocity(0)
self.create_subscription(Twist, '/cmd_vel', self.cmd_vel_callback, 1)
self.get_logger().info('EPuck Initialized')
# Initialize odometry
self.reset_odometry()
self.left_wheel_sensor = self.robot.getPositionSensor(
'left wheel sensor')
self.right_wheel_sensor = self.robot.getPositionSensor(
'right wheel sensor')
self.left_wheel_sensor.enable(self.period.value)
self.right_wheel_sensor.enable(self.period.value)
self.odometry_publisher = self.create_publisher(Odometry, '/odom', 1)
# Initialize IMU
self.gyro = self.robot.getGyro('gyro')
if self.gyro:
self.gyro.enable(self.period.value)
else:
self.get_logger().info('Gyroscope is not present for this e-puck version')
self.accelerometer = self.robot.getAccelerometer('accelerometer')
self.accelerometer.enable(self.period.value)
self.imu_publisher = self.create_publisher(Imu, '/imu', 10)
# Initialize ground sensors
self.ground_sensors = {}
self.ground_sensor_publishers = {}
self.ground_sensor_broadcasters = []
for i in range(3):
idx = 'gs{}'.format(i)
ground_sensor = self.robot.getDistanceSensor(idx)
if ground_sensor:
ground_sensor.enable(self.period.value)
self.ground_sensors[idx] = ground_sensor
self.ground_sensor_publishers[idx] = self.create_publisher(
Range, '/' + idx, 1)
ground_sensor_broadcaster = StaticTransformBroadcaster(self)
ground_sensor_transform = TransformStamped()
ground_sensor_transform.header.stamp = self.now()
ground_sensor_transform.header.frame_id = "base_link"
ground_sensor_transform.child_frame_id = "gs" + str(i)
ground_sensor_transform.transform.rotation = euler_to_quaternion(
0, pi/2, 0)
ground_sensor_transform.transform.translation.x = SENSOR_DIST_FROM_CENTER - 0.005
ground_sensor_transform.transform.translation.y = 0.009 - i * 0.009
ground_sensor_transform.transform.translation.z = 0.0
ground_sensor_broadcaster.sendTransform(
ground_sensor_transform)
self.ground_sensor_broadcasters.append(
ground_sensor_broadcaster)
else:
self.get_logger().info(
'Ground sensor `{}` is not present for this e-puck version'.format(idx))
# Intialize distance sensors
self.distance_sensor_publishers = {}
self.distance_sensors = {}
self.distance_sensor_broadcasters = []
for i in range(8):
sensor = self.robot.getDistanceSensor('ps{}'.format(i))
sensor.enable(self.period.value)
sensor_publisher = self.create_publisher(
Range, '/ps{}'.format(i), 10)
self.distance_sensors['ps{}'.format(i)] = sensor
self.distance_sensor_publishers['ps{}'.format(
i)] = sensor_publisher
distance_sensor_broadcaster = StaticTransformBroadcaster(self)
distance_sensor_transform = TransformStamped()
distance_sensor_transform.header.stamp = self.now()
distance_sensor_transform.header.frame_id = "base_link"
distance_sensor_transform.child_frame_id = "ps" + str(i)
distance_sensor_transform.transform.rotation = euler_to_quaternion(
0, 0, DISTANCE_SENSOR_ANGLE[i])
distance_sensor_transform.transform.translation.x = SENSOR_DIST_FROM_CENTER * \
cos(DISTANCE_SENSOR_ANGLE[i])
distance_sensor_transform.transform.translation.y = SENSOR_DIST_FROM_CENTER * \
sin(DISTANCE_SENSOR_ANGLE[i])
distance_sensor_transform.transform.translation.z = 0.0
distance_sensor_broadcaster.sendTransform(
distance_sensor_transform)
self.distance_sensor_broadcasters.append(
distance_sensor_broadcaster)
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.period.value)
self.tof_publisher = self.create_publisher(Range, '/tof', 1)
self.tof_broadcaster = StaticTransformBroadcaster(self)
tof_transform = TransformStamped()
tof_transform.header.stamp = self.now()
tof_transform.header.frame_id = "base_link"
tof_transform.child_frame_id = "tof"
tof_transform.transform.rotation.x = 0.0
tof_transform.transform.rotation.y = 0.0
tof_transform.transform.rotation.z = 0.0
tof_transform.transform.rotation.w = 1.0
tof_transform.transform.translation.x = SENSOR_DIST_FROM_CENTER
tof_transform.transform.translation.y = 0.0
tof_transform.transform.translation.z = 0.0
self.tof_broadcaster.sendTransform(tof_transform)
else:
self.get_logger().info('ToF sensor is not present for this e-puck version')
# Initialize camera
self.camera = self.robot.getCamera('camera')
self.camera.enable(self.camera_period)
self.camera_publisher = self.create_publisher(
Image, '/image_raw', 10)
self.create_timer(self.camera_period / 1000, self.camera_callback)
self.camera_info_publisher = self.create_publisher(
CameraInfo, '/camera_info', 10)
# Initialize binary LEDs
self.binary_leds = []
self.binary_led_subscribers = []
for i in range(NB_BINARY_LEDS):
index = i * 2
led = self.robot.getLED('led{}'.format(index))
led_subscriber = self.create_subscription(
Bool,
'/led{}'.format(index),
partial(self.on_binary_led_callback, index=i),
10
)
self.binary_leds.append(led)
self.binary_led_subscribers.append(led_subscriber)
# Initialize RGB LEDs
self.rgb_leds = []
self.rgb_led_subscribers = []
for i in range(NB_RGB_LEDS):
index = i * 2 + 1
led = self.robot.getLED('led{}'.format(index))
led_subscriber = self.create_subscription(
Int32,
'/led{}'.format(index),
partial(self.on_rgb_led_callback, index=i),
10
)
self.rgb_leds.append(led)
self.rgb_led_subscribers.append(led_subscriber)
# Initialize Light sensors
self.light_sensors = []
self.light_publishers = []
self.light_sensor_broadcasters = []
for i in range(NB_LIGHT_SENSORS):
light_sensor = self.robot.getLightSensor(f'ls{i}')
light_sensor.enable(self.period.value)
light_publisher = self.create_publisher(Illuminance, f'/ls{i}', 1)
self.light_publishers.append(light_publisher)
self.light_sensors.append(light_sensor)
light_sensor_broadcaster = StaticTransformBroadcaster(self)
light_transform = TransformStamped()
light_transform.header.stamp = self.now()
light_transform.header.frame_id = "base_link"
light_transform.child_frame_id = "ls" + str(i)
light_transform.transform.rotation = euler_to_quaternion(
0, 0, DISTANCE_SENSOR_ANGLE[i])
light_transform.transform.translation.x = SENSOR_DIST_FROM_CENTER * \
cos(DISTANCE_SENSOR_ANGLE[i])
light_transform.transform.translation.y = SENSOR_DIST_FROM_CENTER * \
sin(DISTANCE_SENSOR_ANGLE[i])
light_transform.transform.translation.z = 0.0
light_sensor_broadcaster.sendTransform(light_transform)
self.light_sensor_broadcasters.append(light_sensor_broadcaster)
# Static tf broadcaster: Laser
self.laser_broadcaster = StaticTransformBroadcaster(self)
laser_transform = TransformStamped()
laser_transform.header.stamp = self.now()
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.0
self.laser_broadcaster.sendTransform(laser_transform)
# Main loop
self.create_timer(self.period.value / 1000, self.step_callback)
# Transforms
self.tf_broadcaster = TransformBroadcaster(self)
def on_rgb_led_callback(self, msg, index):
self.rgb_leds[index].set(msg.data)
def on_binary_led_callback(self, msg, index):
value = 1 if msg.data else 0
self.binary_leds[index].set(value)
def reset_odometry(self):
self.prev_left_wheel_ticks = 0
self.prev_right_wheel_ticks = 0
self.prev_position = (0.0, 0.0)
self.prev_angle = 0.0
def on_param_changed(self, params):
result = SetParametersResult()
result.successful = True
for param in params:
if param.name == "wheel_radius":
self.reset_odometry()
self.wheel_radius = param.value
elif param.name == "wheel_distance":
self.reset_odometry()
self.wheel_distance = param.value
return result
def step_callback(self):
self.robot.step(self.period.value)
stamp = self.now()
self.publish_odometry_data(stamp)
self.publish_distance_data(stamp)
self.publish_light_data(stamp)
self.publish_ground_sensor_data(stamp)
def publish_ground_sensor_data(self, stamp):
for idx in self.ground_sensors.keys():
msg = Range()
msg.header.stamp = stamp
msg.header.frame_id = idx
msg.field_of_view = self.ground_sensors[idx].getAperture()
msg.min_range = GROUND_MIN_RANGE
msg.max_range = GROUND_MAX_RANGE
msg.range = interpolate_table(
self.ground_sensors[idx].getValue(), GROUND_TABLE)
msg.radiation_type = Range.INFRARED
self.ground_sensor_publishers[idx].publish(msg)
def cmd_vel_callback(self, twist):
self.get_logger().info('Message received')
left_velocity = (2.0 * twist.linear.x - twist.angular.z *
self.wheel_distance) / (2.0 * self.wheel_radius)
right_velocity = (2.0 * twist.linear.x + twist.angular.z *
self.wheel_distance) / (2.0 * self.wheel_radius)
self.left_motor.setVelocity(left_velocity)
self.right_motor.setVelocity(right_velocity)
def publish_odometry_data(self, stamp):
encoder_period_s = self.period.value / 1000.0
left_wheel_ticks = self.left_wheel_sensor.getValue()
right_wheel_ticks = self.right_wheel_sensor.getValue()
# Calculate velocities
v_left_rad = (left_wheel_ticks -
self.prev_left_wheel_ticks) / encoder_period_s
v_right_rad = (right_wheel_ticks -
self.prev_right_wheel_ticks) / encoder_period_s
v_left = v_left_rad * self.wheel_radius
v_right = v_right_rad * self.wheel_radius
v = (v_left + v_right) / 2
omega = (v_right - v_left) / self.wheel_distance
# Calculate position & angle
# Fourth order Runge - Kutta
# Reference: https://www.cs.cmu.edu/~16311/s07/labs/NXTLabs/Lab%203.html
k00 = v * cos(self.prev_angle)
k01 = v * sin(self.prev_angle)
k02 = omega
k10 = v * cos(self.prev_angle + encoder_period_s * k02 / 2)
k11 = v * sin(self.prev_angle + encoder_period_s * k02 / 2)
k12 = omega
k20 = v * cos(self.prev_angle + encoder_period_s * k12 / 2)
k21 = v * sin(self.prev_angle + encoder_period_s * k12 / 2)
k22 = omega
k30 = v * cos(self.prev_angle + encoder_period_s * k22 / 2)
k31 = v * sin(self.prev_angle + encoder_period_s * k22 / 2)
k32 = omega
position = [
self.prev_position[0] + (encoder_period_s / 6) *
(k00 + 2 * (k10 + k20) + k30),
self.prev_position[1] + (encoder_period_s / 6) *
(k01 + 2 * (k11 + k21) + k31)
]
angle = self.prev_angle + \
(encoder_period_s / 6) * (k02 + 2 * (k12 + k22) + k32)
# Update variables
self.prev_position = position.copy()
self.prev_angle = angle
self.prev_left_wheel_ticks = left_wheel_ticks
self.prev_right_wheel_ticks = right_wheel_ticks
# Pack & publish odometry
msg = Odometry()
msg.header.stamp = stamp
msg.header.frame_id = 'odom'
msg.child_frame_id = 'base_link'
msg.twist.twist.linear.x = v
msg.twist.twist.angular.z = omega
msg.pose.pose.position.x = position[0]
msg.pose.pose.position.y = position[1]
msg.pose.pose.orientation = euler_to_quaternion(0, 0, angle)
self.odometry_publisher.publish(msg)
# Pack & publish transforms
tf = TransformStamped()
tf.header.stamp = stamp
tf.header.frame_id = 'odom'
tf.child_frame_id = 'base_link'
tf.transform.translation.x = position[0]
tf.transform.translation.y = position[1]
tf.transform.translation.z = 0.0
tf.transform.rotation = euler_to_quaternion(0, 0, angle)
self.tf_broadcaster.sendTransform(tf)
def publish_light_data(self, stamp):
for light_publisher, light_sensor in zip(self.light_publishers, self.light_sensors):
msg = Illuminance()
msg.header.stamp = stamp
msg.illuminance = interpolate_table(
light_sensor.getValue(), LIGHT_TABLE) * IRRADIANCE_TO_ILLUMINANCE
msg.variance = 0.1
light_publisher.publish(msg)
def publish_distance_data(self, stamp):
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_table(
self.distance_sensors[key].getValue(), DISTANCE_TABLE)
# Publish range: Infrared
for i, key in enumerate(self.distance_sensors):
msg = Range()
msg.header.stamp = stamp
msg.header.frame_id = key
msg.field_of_view = self.distance_sensors[key].getAperture()
msg.min_range = INFRARED_MIN_RANGE
msg.max_range = INFRARED_MAX_RANGE
msg.range = dists[i]
msg.radiation_type = Range.INFRARED
self.distance_sensor_publishers[key].publish(msg)
# Publish range: ToF
if self.tof_sensor:
dist_tof = interpolate_table(self.tof_sensor.getValue(), TOF_TABLE)
msg = Range()
msg.header.stamp = stamp
msg.header.frame_id = 'tof'
msg.field_of_view = self.tof_sensor.getAperture()
msg.min_range = TOF_MAX_RANGE
msg.max_range = TOF_MIN_RANGE
msg.range = dist_tof
msg.radiation_type = Range.INFRARED
self.tof_publisher.publish(msg)
# 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
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 + INFRARED_MAX_RANGE
msg.ranges = [
dists[3] + SENSOR_DIST_FROM_CENTER, # -150
OUT_OF_RANGE, # -135
OUT_OF_RANGE, # -120
OUT_OF_RANGE, # -105
dists[2] + SENSOR_DIST_FROM_CENTER, # -90
OUT_OF_RANGE, # -75
OUT_OF_RANGE, # -60
dists[1] + SENSOR_DIST_FROM_CENTER, # -45
OUT_OF_RANGE, # -30
dists[0] + SENSOR_DIST_FROM_CENTER, # -15
OUT_OF_RANGE, # 0
dists[7] + SENSOR_DIST_FROM_CENTER, # 15
OUT_OF_RANGE, # 30
dists[6] + SENSOR_DIST_FROM_CENTER, # 45
OUT_OF_RANGE, # 60
OUT_OF_RANGE, # 75
dists[5] + SENSOR_DIST_FROM_CENTER, # 90
OUT_OF_RANGE, # 105
OUT_OF_RANGE, # 120
OUT_OF_RANGE, # 135
dists[4] + SENSOR_DIST_FROM_CENTER, # 150
]
self.laser_publisher.publish(msg)
def imu_callback(self):
gyro_data = self.gyro.getValues()
accelerometer_data = self.accelerometer.getValues()
msg = Imu()
msg.angular_velocity.x = gyro_data[0]
msg.angular_velocity.y = gyro_data[1]
msg.angular_velocity.z = gyro_data[2]
msg.linear_acceleration.x = accelerometer_data[0]
msg.linear_acceleration.y = accelerometer_data[1]
msg.linear_acceleration.z = accelerometer_data[2]
self.imu_publisher.publish(msg)
def camera_callback(self):
# Image data
msg = Image()
msg.height = self.camera.getHeight()
msg.width = self.camera.getWidth()
msg.is_bigendian = False
msg.step = self.camera.getWidth() * 4
msg.data = self.camera.getImage()
msg.encoding = 'bgra8'
self.camera_publisher.publish(msg)
# CameraInfo data
msg = CameraInfo()
msg.header.frame_id = 'camera_frame'
msg.height = self.camera.getHeight()
msg.width = self.camera.getWidth()
msg.distortion_model = 'plumb_bob'
msg.d = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.k = [
self.camera.getFocalLength(), 0.0, self.camera.getWidth() / 2,
0.0, self.camera.getFocalLength(), self.camera.getHeight() / 2,
0.0, 0.0, 1.0
]
msg.p = [
self.camera.getFocalLength(), 0.0, self.camera.getWidth() / 2, 0.0,
0.0, self.camera.getFocalLength(), self.camera.getHeight() / 2, 0.0,
0.0, 0.0, 1.0, 0.0
]
self.camera_info_publisher.publish(msg)
class KheperaDriver(WebotsDifferentialDriveNode):
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 __get_ultrasonic_at_angle(self, angle):
for name, ultrasonic_angle in ULTRASONIC_SENSOR_ANGLE.items():
if ultrasonic_angle == angle:
return self.distance_sensors[name]
return None
def __publish_laserscan_data(self):
stamp = Time(seconds=self.robot.getTime()).to_msg()
lookup_table_infrared = self.distance_sensors[
'front infrared sensor'].getLookupTable()
lookup_table_ultrasonic = self.distance_sensors[
'front ultrasonic sensor'].getLookupTable()
msg = LaserScan()
msg.header.frame_id = 'laser_scanner'
msg.header.stamp = stamp
msg.angle_min = -135 * pi / 180
msg.angle_max = 180 * pi / 180
msg.angle_increment = 45 * pi / 180
msg.range_min = min(lookup_table_infrared[0],
lookup_table_infrared[-3]) + 0.01
msg.range_max = max(lookup_table_ultrasonic[0],
lookup_table_ultrasonic[-3]) - 0.1
for name, angle in DISTANCE_SENSOR_ANGLE.items():
distance = interpolate_lookup_table(
self.distance_sensors[name].getValue(), lookup_table_infrared)
if distance > max(lookup_table_infrared[0],
lookup_table_infrared[-3]) - 0.03:
ultrasonic_sensor = self.__get_ultrasonic_at_angle(angle)
if ultrasonic_sensor:
distance = interpolate_lookup_table(
ultrasonic_sensor.getValue(), lookup_table_ultrasonic)
else:
distance = 0
msg.ranges.append(distance)
self.laser_publisher.publish(msg)
class InterfaceForRobot():
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 wait_for_services(self):
rospy.logwarn("\n--- Wait for services ... ---")
rospy.wait_for_service("/to_HoleCHeck")
rospy.wait_for_service("/to_RobotControl")
rospy.logwarn("\nWhole servers are open!\n")
def publish_init_tf(self, obj_dict):
obj_name_list = obj_dict.keys()
obj_name_list.sort()
for obj_name in obj_name_list:
obj = obj_dict[obj_name]
rospy.logerr(obj_name)
obj_real_pose = obj.real_pose_mat
obj_posestamped_msg = self.get_posestampedMSG_from_mat(
obj_real_pose, "table")
self.send_tf_from_posestampedMSG(obj_posestamped_msg, obj_name)
self.send_holepin_tf(obj)
self.send_grasp_tf(obj)
stl_path = self.get_obj_stl_path(obj.obj_type)
self.scene.add_mesh(obj_name, obj_posestamped_msg, stl_path)
def get_posestampedMSG_from_mat(self, tf_mat, frame_id):
tr = tf.translation_from_matrix(tf_mat)
quat = tf.quaternion_from_matrix(tf_mat)
posestamped = PoseStamped()
posestamped.header.stamp = rospy.Time.now()
posestamped.header.frame_id = frame_id
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_obj_stl_path(self, obj_type):
stl_path = os.path.join(self.stl_pkg_dir, "urdfs",
obj_type + ".SLDPRT", "meshes",
obj_type + ".SLDPRT.STL")
return stl_path
def update_just_parent_tf_stl(self, obj):
obj_real_pose = obj.real_pose_mat
obj_posestamped_msg = self.get_posestampedMSG_from_mat(
obj_real_pose, "table")
self.send_tf_from_posestampedMSG(obj_posestamped_msg, obj.obj_name)
stl_path = self.get_obj_stl_path(obj.obj_type)
self.scene.add_mesh(obj.obj_name, obj_posestamped_msg, stl_path)
def send_tf_from_posestampedMSG(self, posestamped, target_name):
t = TransformStamped()
t.child_frame_id = target_name
t.header.stamp = rospy.Time.now()
t.header.frame_id = posestamped.header.frame_id
t.transform.translation.x = posestamped.pose.position.x
t.transform.translation.y = posestamped.pose.position.y
t.transform.translation.z = posestamped.pose.position.z
t.transform.rotation.x = posestamped.pose.orientation.x
t.transform.rotation.y = posestamped.pose.orientation.y
t.transform.rotation.z = posestamped.pose.orientation.z
t.transform.rotation.w = posestamped.pose.orientation.w
print(target_name)
self.br.sendTransform(t)
def send_holepin_tf(self, obj):
for holepin_name, holepin_info in obj.assem_HolePins.items():
holepin_end_mat = holepin_info["end_coordinate"]
holepin_end_posestamped = self.get_posestampedMSG_from_mat(
holepin_end_mat, obj.obj_name)
self.send_tf_from_posestampedMSG(holepin_end_posestamped,
holepin_name + "_end")
holepin_entry_mat = holepin_info["entry_coordinate"]
holepin_entry_posestamped = self.get_posestampedMSG_from_mat(
holepin_entry_mat, obj.obj_name)
self.send_tf_from_posestampedMSG(holepin_entry_posestamped,
holepin_name + "_entry")
def send_grasp_tf(self, obj):
target_obj_type = obj.obj_type
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_mat = self.get_mat_from_tr_rot(grasp_tr, grasp_rot)
grasp_posestamped = self.get_posestampedMSG_from_mat(
grasp_mat, obj.obj_name)
grasp_name = "{}-{}-{}".format(obj.obj_name, "GRASP", idx)
self.send_tf_from_posestampedMSG(grasp_posestamped, grasp_name)
idx += 1
def get_mat_from_tr_rot(self, xyz, rot):
tr_mat = tf.translation_matrix(xyz)
if len(rot) == 3:
rot_mat = tf.euler_matrix(rot[0], rot[0], rot[0])
elif len(rot) == 4:
rot_mat = tf.quaternion_matrix(rot)
else:
print("Given rot component's number is wrong!")
return TypeError
tf_mat = tf.concatenate_matrices(tr_mat, rot_mat)
return tf_mat
def send_msg_for_RobotControl(self, \
assembly_type, parent_obj_name, parent_tf_names, child_obj_name, child_tf_names):
parent = Asm_test([parent_obj_name], parent_tf_names)
child = Asm_test([child_obj_name], child_tf_names)
rospy.logwarn("\n--- Send msg to RobotControl")
rospy.logwarn(assembly_type)
rospy.logwarn(parent)
rospy.logwarn(child)
resp = self.client_for_RobotControl(assembly_type, parent, child)
rospy.logwarn("\n--- Got resp from RobotControl")
return resp
def send_msg_for_HoleCheck(self, \
assembly_type, parent_obj_name, parent_tf_names, child_obj_name, child_tf_names):
parent = Asm_test([parent_obj_name], parent_tf_names)
child = Asm_test([child_obj_name], child_tf_names)
rospy.logwarn("\n--- Send msg to HoleCheck")
rospy.logwarn(assembly_type)
rospy.logwarn(parent)
rospy.logwarn(child)
resp = self.client_for_HoleCheck(assembly_type, parent, child)
rospy.logwarn("\n--- Got resp from HoleCheck")
return resp
def get_xyz_quat_from_tfpose(self, tfpose):
x = tfpose.TransStamped.transform.translation.x
y = tfpose.TransStamped.transform.translation.y
z = tfpose.TransStamped.transform.translation.z
rx = tfpose.TransStamped.transform.rotation.x
ry = tfpose.TransStamped.transform.rotation.y
rz = tfpose.TransStamped.transform.rotation.z
rw = tfpose.TransStamped.transform.rotation.w
tr = [x, y, z]
quat = [rx, ry, rz, rw]
return tr, quat
def send_redetected_holepin_tf(self, obj, holepin_names, criterion):
for holepin_name, cri in zip(holepin_names, criterion):
target_holepin = obj.assem_HolePins[holepin_name]
target_mat = target_holepin[cri + "_coordinate"]
target_posestamped = self.get_posestampedMSG_from_mat(
target_mat, obj.obj_name)
self.send_tf_from_posestampedMSG(target_posestamped,
holepin_name + "_" + cri)
def send_component_tf(self, parent_obj, sub_obj):
pre_comp_list = set(parent_obj.components.keys())
new_comp_list = set(sub_obj.components.keys())
target_comp_list = list(new_comp_list - pre_comp_list)
for child_obj_name in target_comp_list:
target_component = sub_obj.components[child_obj_name]
target_tr = target_component["tr"]
target_quat = target_component["quat"]
target_mat = sub_obj.get_tf_matrix(target_tr, target_quat)
target_posestamped = self.get_posestampedMSG_from_mat(
target_mat, sub_obj.obj_name)
self.send_tf_from_posestampedMSG(target_posestamped,
child_obj_name)
child_obj_type = child_obj_name.split("_")[0]
stl_path = self.get_obj_stl_path(child_obj_type)
self.scene.add_mesh(child_obj_name, target_posestamped, stl_path)
def update_component_stl(self, obj):
target_comp_list = obj.components.keys()
for child_obj_name in target_comp_list:
target_component = obj.components[child_obj_name]
target_tr = target_component["tr"]
target_quat = target_component["quat"]
target_mat = obj.get_tf_matrix(target_tr, target_quat)
target_posestamped = self.get_posestampedMSG_from_mat(
target_mat, obj.obj_name)
self.send_tf_from_posestampedMSG(target_posestamped,
child_obj_name)
child_obj_type = child_obj_name.split("_")[0]
stl_path = self.get_obj_stl_path(child_obj_type)
self.scene.add_mesh(child_obj_name, target_posestamped, stl_path)
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 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 ToolTransformHandler:
"""
This class uses a tfBuffer to handle transforms related to the tools.
"""
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 __callback_set_tcp(self, req):
self.__enable_tcp = True
t = self.empty_transform()
if req.orientation == Quaternion():
t.transform.rotation = Quaternion(*quaternion_from_euler(
req.rpy.roll, req.rpy.pitch, req.rpy.yaw))
else:
t.transform.rotation = req.orientation
t.transform.translation.x = req.position.x
t.transform.translation.y = req.position.y
t.transform.translation.z = req.position.z
self.set_tcp(t)
return CommandStatus.SUCCESS, "Success"
def __callback_reset_tcp(self, _):
self.set_tcp(copy.deepcopy(self.__tool_transform))
return CommandStatus.SUCCESS, "Success"
def __callback_enable_tcp(self, req):
self.__enable_tcp = req.value
self.__send_tcp_transform(None)
self.__publish_tcp_transform()
return CommandStatus.SUCCESS, "Success"
def set_tool(self, tool_transform):
"""
Updates the transform object_base -> tool_link_target in local tfBuffer
:param grip:
"""
self.__tool_transform = tool_transform
return self.set_tcp(copy.deepcopy(self.__tool_transform))
def set_tcp(self, tcp_transform):
"""
Updates the transform object_base -> tool_link_target in local tfBuffer
:param grip:
"""
self.__tcp_transform = tcp_transform
self.__tf_buffer.set_transform(self.__tcp_transform,
"default_authority")
self.__send_tcp_transform(None)
self.__publish_tcp_transform()
return True
def __publish_tcp_transform(self):
msg = TCP()
msg.enabled = self.__enable_tcp
if self.__enable_tcp:
msg.position.x = self.__tcp_transform.transform.translation.x
msg.position.y = self.__tcp_transform.transform.translation.y
msg.position.z = self.__tcp_transform.transform.translation.z
msg.orientation = self.__tcp_transform.transform.rotation
msg.rpy.roll, msg.rpy.pitch, msg.rpy.yaw = euler_from_quaternion([
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
])
else:
msg.orientation = Quaternion(0, 0, 0, 1)
self.__tcp_publisher.publish(msg)
def __send_tcp_transform(self, _):
self.__static_broadcaster.sendTransform(
self.__tcp_transform if self.__enable_tcp else self.
empty_transform())
@staticmethod
def transform_from_dict(dict_):
t = TransformStamped()
t.transform.translation.x = dict_["translation"][0]
t.transform.translation.y = dict_["translation"][1]
t.transform.translation.z = dict_["translation"][2]
t.transform.rotation.x = dict_["quaternion"][0]
t.transform.rotation.y = dict_["quaternion"][1]
t.transform.rotation.z = dict_["quaternion"][2]
t.transform.rotation.w = dict_["quaternion"][3]
t.header.frame_id = "tool_link"
t.child_frame_id = "TCP"
return t
@staticmethod
def empty_transform():
t = TransformStamped()
t.transform.rotation = Quaternion(0, 0, 0, 1)
t.header.frame_id = "tool_link"
t.child_frame_id = "TCP"
return t
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 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/PointCloud` 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
# Create topics
if wb_device.getNumberOfLayers() > 1:
wb_device.enablePointCloud()
self.__publisher = node.create_publisher(PointCloud,
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 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):
points = self._wb_device.getPointCloud()
if points:
msg = PointCloud()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.points = [
Point32(x=point.x, y=point.y, z=point.z) for point in points
]
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)