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)