def main(args=None): topic = '/ifm3d/camera/unit_vectors' topic_type = Image rclpy.init(args=args) qos_profile = QoSProfile( depth=1, reliability=QoSReliabilityPolicy.RELIABLE, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL ) node = Node('uvec_listener') sub = node.create_subscription( topic_type, topic, lambda msg: print(msg), qos_profile) try: rclpy.spin(node) except KeyboardInterrupt: node.destroy_node() rclpy.shutdown() finally: return 0
def main(): rclpy.init() node = Node("talker") pub = node.create_publisher(Pose2D, 'robot0/target_velocity') simTime = SimTimer(True, "veranda/timestamp", node) # Factor to scale down speed by speedScale = 10 # Tick time at 10 hz dt = 0.1 def cb(): # Calculate wheel velocities for current time x, y = speedScale * math.sin( simTime.global_time()), speedScale * 2 * math.cos( 2 * simTime.global_time()) t = 0. print(x, y) # Publish velocities publishTargetVelocity(pub, x, y, t) simTime.create_timer(dt, cb) rclpy.spin(node) node.destroy_node() rclpy.shutdown()
def main(): rclpy.init() node = Node("talker") publeft = node.create_publisher(Float32, 'robot0/left_wheel') pubright = node.create_publisher(Float32, 'robot0/right_wheel') simTime = SimTimer(True, "veranda/timestamp", node) # Factor to scale down speed by speedScale = 2 # Tick time at 10 hz dt = 0.1 def cb(): # Calculate wheel velocities for current time phi1, phi2 = DD_IK(x_t, y_t, simTime.global_time() + 2*math.pi, speedScale) print(phi1, phi2) # Publish velocities publishWheelVelocity(publeft, pubright, phi1, phi2) simTime.create_timer(dt, cb) rclpy.spin(node) node.destroy_node() rclpy.shutdown()
def mainLoop(device): #Gripper is a 2F with a TCP connection gripper = robotiq_2f_gripper_control.baseRobotiq2FGripper.robotiqbaseRobotiq2FGripper() gripper.client = robotiq_modbus_rtu.comModbusRtu.communication() #We connect to the address received as an argument gripper.client.connectToDevice(device) node = Node('robotiq2FGripper') #The Gripper status is published on the topic named 'Robotiq2FGripperRobotInput' pub = node.create_publisher(inputMsg, 'Robotiq2FGripperRobotInput', 1) #The Gripper command is received from the topic named 'Robotiq2FGripperRobotOutput' sud = node.create_subscription(outputMsg, 'Robotiq2FGripperRobotOutput', gripper.refreshCommand, 1) #We loop while rclpy.ok(): #Get and publish the Gripper status status = gripper.getStatus() pub.publish(status) #Wait a little #rospy.sleep(0.05) #Send the most recent command gripper.sendCommand() #Wait a little #rospy.sleep(0.05) node.destroy_node() rclpy.shutdown()
def wait_for_messages(self, themes): # tests_params = {<name>: {'callback', 'topic', 'msg_type', 'internal_params'}} self.func_data = dict([[theme_name, {}] for theme_name in themes]) node = Node('wait_for_messages') for theme_name in themes: theme = self.themes[theme_name] node.get_logger().info('Subscribing %s on topic: %s' % (theme_name, theme['topic'])) self.func_data[theme_name]['sub'] = node.create_subscription( theme['msg_type'], theme['topic'], theme['callback'](theme_name), qos.qos_profile_sensor_data) self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, node) self.prev_time = time.time() break_timeout = False while not break_timeout: rclpy.spin_once(node, timeout_sec=1) if self.timeout > 0 and time.time() - self.prev_time > self.timeout: break_timeout = True self.unregister_all(node, self.func_data) node.destroy_node() return self.func_data
def main(args=None): rclpy.init(args=args) node = Node("task_power_rune2019_client") cli = node.create_client(SetMode, "task_power_rune/set_mode") while not cli.wait_for_service(timeout_sec=1.0): node.get_logger().info('service not available, waiting again...') print(msg) old_attr = termios.tcgetattr(sys.stdin) tty.setcbreak(sys.stdin.fileno()) while rclpy.ok(): if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]: key=sys.stdin.read(1) print(key) if (key == 'q' or key == 'Q'): set_mode_req(cli,0x00) elif(key=='w' or key=='W'): set_mode_req(cli,0x01) elif(key=='e' or key=='E'): set_mode_req(cli,0x02) elif(key=='r' or key=='R'): set_mode_req(cli,0x03) elif(key=='a' or key=='A'): set_mode_req(cli,0x10) elif(key=='s' or key=='S'): set_mode_req(cli,0x11) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_attr) node.destroy_node() rclpy.shutdown()
def main(args=None): rclpy.init(args=args) node = Node('referee_system') referee_system = SimpleRefereeSystem(node) rclpy.spin(node) referee_system node.destroy_node() rclpy.shutdown()
def main(args=None): rclpy.init(args=args) node = Node("simple_node") rclpy.spin(node) node.destroy_node() rclpy.shutdown()
def Robotiq2FGripperStatusListener(): """Initialize the node and subscribe to the Robotiq2FGripperRobotInput topic.""" rclpy.init() node = Node('Robotiq2FGripperStatusListener') node.create_subscription(inputMsg, "Robotiq2FGripperRobotInput", printStatus, 1) try: rclpy.spin(node) except KeyboardInterrupt: pass node.destroy_node() rclpy.shutdown()
def main(): rclpy.init() node = Node('cartesian_compute') try: c = CartesianCompute(node) rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown() return 0
def main(): rclpy.init() node = Node('Controller') pub = node.create_publisher(String, 'DIRECTION', 10) print("W--UP\nA--LEFT\nD--RIGHT\nS--DOWN\nQ--STOP") ip = input() msg = String() while ip != 'Z': if (ip == "W" or ip == "S" or ip == "A" or ip == "D" or ip == "Q"): msg.data = ip pub.publish(msg) else: print("W--UP\nA--LEFT\nD--RIGHT\nS--DOWN\nQ--STOP") ip = input() node.destroy_node() rclpy.shutdown()
def main(args=None): rclpy.init(args=None) # needed to init rclcpp ros for moveit_bindings node = Node("bitbots_localization_handler", automatically_declare_parameters_from_overrides=True) dsd = init(node) node.create_timer(1 / 25.0, dsd.update) multi_executor = MultiThreadedExecutor() multi_executor.add_node(node) try: multi_executor.spin() except KeyboardInterrupt: pass node.destroy_node()
def rosinit(self): node_name = 'circle' rclpy.init() node = Node(node_name) msg = Float32() print('Spinning: {}'.format(node_name)) self.publeft = node.create_publisher(Float32, '/wheel_left') self.pubright = node.create_publisher(Float32, '/wheel_right') if self.problem == 'triangle': self.goal_pt = next(self.tri_pts) self.goal_pt = next(self.tri_pts) self.goal_theta = next(self.tri_thetas) self.goal_theta = next(self.tri_thetas) msg.data = 1.0 self.publeft.publish(msg) msg.data = -msg.data self.pubright.publish(msg) node.create_subscription(Pose2D, '/GPS', self.tri_callback) elif self.problem == 'square': self.goal_pt = next(self.sqr_pts) self.goal_pt = next(self.sqr_pts) self.goal_theta = next(self.sqr_thetas) self.goal_theta = next(self.sqr_thetas) msg.data = 1.0 self.publeft.publish(msg) msg.data = -msg.data self.pubright.publish(msg) node.create_subscription(Pose2D, '/GPS', self.sqr_callback) else: # Circle msg.data = 5.0 self.publeft.publish(msg) msg.data = msg.data * (15.0 + 1.2) / 15 self.pubright.publish(msg) try: rclpy.spin(node) except KeyboardInterrupt: print('Shutting down...') msg.data = 0.0 self.publeft.publish(msg) self.pubright.publish(msg) node.destroy_node() rclpy.shutdown()
def main(): args = sys.argv if len(args) != 2 and len(args) != 3: print("Usage: joystick_differential {topic-in} [topic-out]") print("Joystick should be {topic-in}/joystick") print( "Wheels will be {topic-out}/steer, {topic-out}/left_wheel, and {topic-out}/right_wheel" ) print("If no {topic-out} given, {topic-in} will be used for both") return channelin = channelout = args[1] if len(args) == 3: channelout = args[2] channelin = channelin + "/joystick" rclpy.init(args=args) node = Node("joystick_differential") publeft = node.create_publisher(Float32, channelout + "/left_wheel") pubright = node.create_publisher(Float32, channelout + "/right_wheel") pubsteer = node.create_publisher(Float32, channelout + "/steer") def joystick_callback(msg): if len(msg.axes) < 2: print("Not enough joystick axes") return speed = msg.axes[1] steer = -msg.axes[0] phi1 = phi2 = 1 / R * speed * SPEED phi3 = math.pi / 4.0 * steer publishWheelVelocity(publeft, pubright, phi1, phi2) publishWheelSteer(pubsteer, phi3) node.create_subscription(Joy, channelin, joystick_callback) rclpy.spin(node) node.destroy_node() rclpy.shutdown()
def main(): args = sys.argv if len(args) != 2: print("Usage: joystick_listener {topic}") return channel = str(args[1]) print("Listening on topic \'" + channel + "\'") rclpy.init(args=args) node = Node("joystick_snooper") node.create_subscription(Joy, channel, joystick_callback) rclpy.spin(node) node.destroy_node() rclpy.shutdown()
def main(): args = sys.argv if len(args) != 2: print("Usage: lidar_listener topic") return channel = str(args[1]) print("Listening on topic \'" + channel + "\'") rclpy.init(args=args) node = Node("lidar_snooper") node.create_subscription(LaserScan, channel, lidar_callback) rclpy.spin(node) node.destroy_node() rclpy.shutdown()
def decorated_function(): #SETUP print("Setup fixture") rclpy.init() node = Node('minimal_publisher') pub1 = node.create_publisher(String, 'topic1', latching_qos) msg1 = String() msg1.data = "hello1" node.get_logger().info("Publishing \"{}\" on topic \"{}\"".format("hello1", "topic1")) pub1.publish(msg1) pub2 = node.create_publisher(String, 'topic2', latching_qos) msg2 = String() msg2.data = "hello2" node.get_logger().info("Publishing \"{}\" on topic \"{}\"".format("hello2", "topic2")) pub2.publish(msg2) pub3 = node.create_publisher(String, 'topic3', latching_qos) msg3 = String() msg3.data = "hello3" node.get_logger().info("Publishing \"{}\" on topic \"{}\"".format("hello3", "topic3")) pub3.publish(msg3) exe = SingleThreadedExecutor() exe.add_node(node) future_message = Future() #TEST result = function(future_message, exe) #TEARDOWN print("Teardown fixture") node.destroy_node() rclpy.shutdown() time.sleep(0.2) return result
def __init__(self): node = Node('ros2top') # spin on another thread thread = Thread(target=rclpy.spin, args=(node, ), daemon=True) thread.start() self.model = ScenesModel(node) start_scene = None while rclpy.ok(): try: Screen.wrapper(self.screen_main, arguments=[start_scene]) break except ResizeScreenError as e: start_scene = e.scene node.destroy_node() rclpy.shutdown()
def main(args=None): rclpy.init(args=args) nh = Node('webserver') nh.declare_parameter('port') os.chdir( os.path.dirname(__file__) + '/../../../../share/pimouse_control2/contents') param = nh.get_parameter_or('port') if param.type_ == Parameter.Type.NOT_SET: port = 8000 else: port = param.value handler = http.server.SimpleHTTPRequestHandler with socketserver.TCPServer(('', port), handler) as httpd: print('serving at port', port) httpd.serve_forever() nh.destroy_node() rclpy.shutdown()
def publisher(args=None): """Main loop which requests new commands and publish them on the Robotiq2FGripperRobotOutput topic.""" rclpy.init(args=args) node = Node('Robotiq2FGripperSimpleController') pub = node.create_publisher(outputMsg, 'Robotiq2FGripperRobotOutput', 1) command = outputMsg() try: while rclpy.ok(): command = genCommand(askForCommand(command), command) pub.publish(command) sleep(0.1) except KeyboardInterrupt: pass node.destroy_node() rclpy.shutdown()
class ROS2TopicManager(object): """ """ ## # @if jp # @brief コンストラクタ # # コンストラクタ # # @param self # # @else # @brief Constructor # # @param self # # @endif def __init__(self): self._thread = None self._loop = True #mgr = OpenRTM_aist.Manager.instance() # mgr.addManagerActionListener(ManagerActionListener(self)) #self._rtcout = mgr.getLogbuf("ROS2TopicManager") ## # @if jp # @brief デストラクタ # # # @param self # # @else # # @brief self # # @endif def __del__(self): pass ## # @if jp # @brief ROS2初期化 # # @param self # @param args rclpy.initの引数 # # @else # # @brief # # @param self # @param args # # @endif def start(self, args=[]): rclpy.init(args=args) self._node = Node("openrtm") def spin(): while self._loop: rclpy.spin_once(self._node, timeout_sec=0.01) self._thread = threading.Thread(target=spin) self._thread.daemon = True self._thread.start() ## # @if jp # @brief 終了処理 # # @param self # # @else # # @brief # # @param self # # @endif def shutdown(self): if self._node: self._loop = False self._node.destroy_node() # rclpy.try_shutdown() # if self._thread: # self._thread.join() ## # @if jp # @brief Publisherオブジェクト生成 # # @param self # @param msgtype メッセージ型 # @param topic トピック名 # @return Publisherオブジェクト # # @else # # @brief # # @param self # @param msgtype # @param topic # @return # # @endif def createPublisher(self, msgtype, topic): global mutex guard = OpenRTM_aist.ScopedLock(mutex) if self._node: return self._node.create_publisher(msgtype, topic) return None ## # @if jp # @brief Subscriberオブジェクト生成 # # @param self # @param msgtype メッセージ型 # @param topic トピック名 # @param listener コールバック関数 # @return Subscriberオブジェクト # # @else # # @brief # # @param self # @param msgtype # @param topic # @param listener # @return # # @endif def createSubscriber(self, msgtype, topic, listener): global mutex guard = OpenRTM_aist.ScopedLock(mutex) if self._node: return self._node.create_subscription(msgtype, topic, listener) return None def deletePublisher(self, pub): pass def deleteSubscriber(self, sub): pass ## # @if jp # @brief インスタンス取得 # # @return インスタンス # # @else # # @brief # # @return インスタンス # # @endif def instance(args=[]): global manager global mutex guard = OpenRTM_aist.ScopedLock(mutex) if manager is None: manager = ROS2TopicManager() manager.start(args) return manager instance = staticmethod(instance) ## # @if jp # @brief ROS2TopicManagerを初期化している場合に終了処理を呼び出す # # # @else # # @brief # # # @endif def shutdown_global(): global manager global mutex guard = OpenRTM_aist.ScopedLock(mutex) if manager is not None: manager.shutdown() manager = None shutdown_global = staticmethod(shutdown_global)
def get_hit(msg): hits = msg.data for i, hit in enumerate(hits): hit = struct.unpack('b', hit)[0] if hit: print(f'Contact on sensor: {i}') ros.init() node = Node('Circle') left_pub = node.create_publisher(Float32, 'robot0/left_wheel') right_pub = node.create_publisher(Float32, 'robot0/right_wheel') contact_sub = node.create_subscription(ByteMultiArray, 'robot0/touches', get_hit) msg = Float32() msg.data = 5.0 left_pub.publish(msg) msg.data = 10.0 right_pub.publish(msg) try: ros.spin(node) except KeyboardInterrupt: pass node.destroy_node() ros.shutdown()
class Ui(QtWidgets.QMainWindow): def __init__(self): super(Ui, self).__init__() # Initialize UI self.load_ui() self.reset_ui_state() # Init state self.recording_enabled_target = False # Create ROS self.node = Node("asv_status") # Init ROS Entities self.init_ros() # Create Qt timer to poll ROS2 node for events self.ros_timer = QtCore.QTimer(self) self.ros_timer.timeout.connect(self.handle_ros) self.ros_timer.start(10) # Connect Checkbox signal to publish ROS message with updated state self.checkbox_recording_enabled.stateChanged.connect( self.update_recording_enable_target) # Display window self.show() def update_recording_enable_target(self, state): if (QtCore.Qt.Checked == state): self.recording_enabled_target = True else: self.recording_enabled_target = False msg = Bool() msg.data = self.recording_enabled_target self.pub_req_enable_recording.publish(msg) def init_ros(self): # Create ROS Timer for vehicle heartbeat self.heartbeat_received = False self.last_heartbeat_time = datetime.datetime.now() self.heartbeat_timer = self.node.create_timer(1, self.heartbeat_callback) # Create QOS profiles qos_be = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) qos_klr_v = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE, durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE) qos_klr_tl = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) # Create Subscribers self.sub_heartbeat = self.node.create_subscription( msg_type=String, topic='heartbeat', callback=self.update_heartbeat, qos_profile=qos_klr_v) self.sub_camera_status = self.node.create_subscription( msg_type=String, topic='camera_status', callback=self.update_camera_status, qos_profile=qos_klr_tl) self.sub_gps_status = self.node.create_subscription( msg_type=String, topic='gps_status', callback=self.update_gps_status, qos_profile=qos_klr_tl) self.sub_storage_status = self.node.create_subscription( msg_type=String, topic='storage_status', callback=self.update_storage_status, qos_profile=qos_klr_tl) self.sub_system_time = self.node.create_subscription( msg_type=String, topic='system_time', callback=self.update_system_time, qos_profile=qos_klr_tl) self.sub_recording_status = self.node.create_subscription( msg_type=String, topic='recording_status', callback=self.update_recording_status, qos_profile=qos_klr_tl) self.sub_recorded_samples = self.node.create_subscription( msg_type=String, topic='recorded_samples', callback=self.update_recorded_samples, qos_profile=qos_klr_tl) self.sub_video_left = self.node.create_subscription( msg_type=CompressedImage, topic='camera_left/image_raw/compressed', callback=self.update_video_image, qos_profile=qos_be) # Create publisher self.pub_req_enable_recording = self.node.create_publisher( msg_type=Bool, topic='recording_enabled_target', qos_profile=qos_klr_tl) # Set initial recording enable target msg = Bool() msg.data = self.recording_enabled_target self.pub_req_enable_recording.publish(msg) def load_ui(self): # Get path to Qt UI file package_share_directory = get_package_share_directory('asv_status') ui_path = os.path.join(package_share_directory, "ui/asv_status.ui") # Load UI uic.loadUi(ui_path, self) # Find UI elements self.label_camera_status = self.findChild(QtWidgets.QLabel, 'label_camera_status') self.label_gps_status = self.findChild(QtWidgets.QLabel, 'label_gps_status') self.label_storage_status = self.findChild(QtWidgets.QLabel, 'label_storage_status') self.label_system_time = self.findChild(QtWidgets.QLabel, 'label_system_time') self.label_recording_status = self.findChild(QtWidgets.QLabel, 'label_recording_status') self.label_recorded_samples = self.findChild(QtWidgets.QLabel, 'label_recorded_samples') self.label_video = self.findChild(QtWidgets.QLabel, 'label_video') self.checkbox_recording_enabled = self.findChild( QtWidgets.QCheckBox, 'checkbox_recording_enabled') def reset_ui_state(self): # Set starting values for labels self.update_camera_status(String(data="UNKNOWN")) self.update_gps_status(String(data="UNKNOWN")) self.update_storage_status(String(data="UNKNOWN")) self.update_system_time(String(data="UNKNOWN")) self.update_recording_status(String(data="NOT READY")) self.update_recorded_samples(String(data="0")) # Reset video label self.label_video.clear() self.label_video.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignBottom) self.label_video.setScaledContents(True) self.label_video.setText("DISCONNECTED") def cleanup(self): self.node.destroy_node() def handle_ros(self): rclpy.spin_once(node=self.node, timeout_sec=0) def heartbeat_callback(self): # If we haven't received a new heartbeat message in the last 3 seconds, clear the video feed elapsed_ms = (datetime.datetime.now() - self.last_heartbeat_time).total_seconds() * 1000 if (elapsed_ms > 3000): self.last_heartbeat_time = datetime.datetime.now() if self.heartbeat_received is False: self.label_video.clear() self.label_video.setText("DISCONNECTED") else: self.label_video.setText("") self.heartbeat_received = False # Message handlers def update_camera_status(self, msg): self.camera_status = msg.data self.label_camera_status.setText(msg.data) def update_gps_status(self, msg): self.gps_status = msg.data self.label_gps_status.setText(msg.data) def update_storage_status(self, msg): self.storage_status = msg.data self.label_storage_status.setText(msg.data) def update_system_time(self, msg): self.system_time = msg.data self.label_system_time.setText(msg.data) def update_recording_status(self, msg): self.recording_status = msg.data self.label_recording_status.setText(msg.data) def update_recorded_samples(self, msg): self.recorded_samples = msg.data self.label_recorded_samples.setText(msg.data) def update_heartbeat(self, msg): self.heartbeat_received = True def update_video_image(self, msg): qp = QPixmap() if qp.loadFromData(msg.data): self.label_video.setPixmap(qp)
class ROS2TopicManager(object): """ """ ## # @if jp # @brief コンストラクタ # # コンストラクタ # # @param self # # @else # @brief Constructor # # @param self # # @endif def __init__(self): self._thread = None self._loop = True #mgr = OpenRTM_aist.Manager.instance() # mgr.addManagerActionListener(ManagerActionListener(self)) #self._rtcout = mgr.getLogbuf("ROS2TopicManager") ## # @if jp # @brief デストラクタ # # # @param self # # @else # # @brief self # # @endif def __del__(self): pass ## # @if jp # @brief ROS2初期化 # # @param self # @param args rclpy.initの引数 # # @else # # @brief # # @param self # @param args # # @endif def start(self, args=[]): rclpy.init(args=args) self._node = Node("openrtm") def spin(): while self._loop: rclpy.spin_once(self._node, timeout_sec=0.01) self._thread = threading.Thread(target=spin) self._thread.daemon = True self._thread.start() ## # @if jp # @brief 終了処理 # # @param self # # @else # # @brief # # @param self # # @endif def shutdown(self): if self._node: self._loop = False self._node.destroy_node() # rclpy.try_shutdown() # if self._thread: # self._thread.join() ## # @if jp # @brief Publisherオブジェクト生成 # # @param self # @param msgtype メッセージ型 # @param topic トピック名 # @param qos QoSProfile # @return Publisherオブジェクト # # @else # # @brief # # @param self # @param msgtype # @param topic # @param qos # @return # # @endif def createPublisher(self, msgtype, topic, qos=None): global mutex if qos is None: qos = QoSProfile(depth=10) guard = OpenRTM_aist.ScopedLock(mutex) if self._node: return self._node.create_publisher(msgtype, topic, qos) return None ## # @if jp # @brief Subscriberオブジェクト生成 # # @param self # @param msgtype メッセージ型 # @param topic トピック名 # @param listener コールバック関数 # @param qos QoSProfile # @return Subscriberオブジェクト # # @else # # @brief # # @param self # @param msgtype # @param topic # @param listener # @param qos # @return # # @endif def createSubscriber(self, msgtype, topic, listener, qos=None): global mutex if qos is None: qos = QoSProfile(depth=10) guard = OpenRTM_aist.ScopedLock(mutex) if self._node: return self._node.create_subscription(msgtype, topic, listener, qos) return None def deletePublisher(self, pub): pass def deleteSubscriber(self, sub): pass ## # @if jp # @brief インスタンス取得 # # @return インスタンス # # @else # # @brief # # @return インスタンス # # @endif def instance(args=[]): global manager global mutex guard = OpenRTM_aist.ScopedLock(mutex) if manager is None: manager = ROS2TopicManager() manager.start(args) return manager instance = staticmethod(instance) ## # @if jp # @brief ROS2TopicManagerを初期化している場合に終了処理を呼び出す # # # @else # # @brief # # # @endif def shutdown_global(): global manager global mutex guard = OpenRTM_aist.ScopedLock(mutex) if manager is not None: manager.shutdown() manager = None shutdown_global = staticmethod(shutdown_global) ## # @if jp # @brief プロパティからQoSProfileを設定する # @param prop プロパティ # @return QoSProfile # # @else # # @brief # @param prop # @return QoSProfile # # @endif def get_qosprofile(prop): if hasattr(rclpy.qos, "HistoryPolicy"): HistoryPolicy = rclpy.qos.HistoryPolicy else: HistoryPolicy = rclpy.qos.QoSHistoryPolicy if hasattr(rclpy.qos, "Duration"): Duration = rclpy.qos.Duration else: Duration = rclpy.qos.QoSDuration if hasattr(rclpy.qos, "ReliabilityPolicy"): ReliabilityPolicy = rclpy.qos.ReliabilityPolicy else: ReliabilityPolicy = rclpy.qos.QoSReliabilityPolicy if hasattr(rclpy.qos, "DurabilityPolicy"): DurabilityPolicy = rclpy.qos.DurabilityPolicy else: DurabilityPolicy = rclpy.qos.QoSDurabilityPolicy if hasattr(rclpy.qos, "LivelinessPolicy"): LivelinessPolicy = rclpy.qos.LivelinessPolicy else: LivelinessPolicy = rclpy.qos.QoSLivelinessPolicy history = HistoryPolicy.KEEP_LAST history_type = prop.getProperty("history", "KEEP_LAST") if history_type == "SYSTEM_DEFAULT": history = HistoryPolicy.SYSTEM_DEFAULT elif history_type == "KEEP_ALL": history = HistoryPolicy.KEEP_ALL else: history = HistoryPolicy.KEEP_LAST depth = 10 depth_value_str = prop.getProperty("depth", "10") try: depth = int(depth_value_str) except ValueError: pass reliability = ReliabilityPolicy.RELIABLE reliability_type = prop.getProperty("reliability", "RELIABLE") if reliability_type == "SYSTEM_DEFAULT": reliability = ReliabilityPolicy.SYSTEM_DEFAULT elif reliability_type == "BEST_EFFORT": reliability = ReliabilityPolicy.BEST_EFFORT else: reliability = ReliabilityPolicy.RELIABLE durability = DurabilityPolicy.VOLATILE durability_type = prop.getProperty("durability", "VOLATILE") if durability_type == "SYSTEM_DEFAULT": durability = DurabilityPolicy.SYSTEM_DEFAULT elif durability_type == "TRANSIENT_LOCAL": durability = DurabilityPolicy.TRANSIENT_LOCAL else: durability = DurabilityPolicy.VOLATILE lifespan = Duration(seconds=0, nanoseconds=0) lifespan_value_str = prop.getProperty("lifespan", "0") try: lifespan_value = int(lifespan_value_str) lifespan = Duration(nanoseconds=lifespan_value) except ValueError: pass deadline = Duration(seconds=0, nanoseconds=0) deadline_value_str = prop.getProperty("deadline", "0") try: deadline_value = int(deadline_value_str) deadline = Duration(nanoseconds=deadline_value) except ValueError: pass liveliness = LivelinessPolicy.SYSTEM_DEFAULT liveliness_type = prop.getProperty("liveliness", "SYSTEM_DEFAULT") if liveliness_type == "AUTOMATIC": liveliness = LivelinessPolicy.AUTOMATIC elif liveliness_type == "MANUAL_BY_NODE": liveliness = LivelinessPolicy.MANUAL_BY_NODE elif liveliness_type == "MANUAL_BY_TOPIC": liveliness = LivelinessPolicy.MANUAL_BY_TOPIC else: liveliness = LivelinessPolicy.SYSTEM_DEFAULT liveliness_lease_duration = Duration(seconds=0, nanoseconds=0) liveliness_lease_duration_value_str = prop.getProperty( "liveliness_lease_duration", "0") try: liveliness_lease_duration_value = int( liveliness_lease_duration_value_str) liveliness_lease_duration = Duration( nanoseconds=liveliness_lease_duration_value) except ValueError: pass avoid_ros_namespace_conventions = False if OpenRTM_aist.toBool( prop.getProperty("avoid_ros_namespace_conventions"), "YES", "NO", False): avoid_ros_namespace_conventions = True else: avoid_ros_namespace_conventions = False qos = QoSProfile( history=history, depth=depth, reliability=reliability, durability=durability, lifespan=lifespan, deadline=deadline, liveliness=liveliness, liveliness_lease_duration=liveliness_lease_duration, avoid_ros_namespace_conventions=avoid_ros_namespace_conventions) return qos get_qosprofile = staticmethod(get_qosprofile)
def main(): args = sys.argv joyname = "" if len(args) > 1: joyname = args[1] else: # Iterate over the joystick devices. print("Available Joysticks:") for fn in os.listdir('/dev/input'): if fn.startswith('js'): print('\t%s' % (fn)) sys.exit() # We'll store the states here. axis_states = {} button_states = {} axis_map = [] button_map = [] # Open the joystick device. fn = '/dev/input/' + joyname print('Opening %s...' % fn) jsdev = open(fn, 'rb') # Get the device name. #buf = bytearray(63) buf = array.array('b', [0] * 64) ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len) js_name = buf.tostring() print('Device name: %s' % js_name) # Get number of axes and buttons. buf = array.array('B', [0]) ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES num_axes = buf[0] buf = array.array('B', [0]) ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS num_buttons = buf[0] # Get the axis map. buf = array.array('B', [0] * 0x40) ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP for axis in buf[:num_axes]: axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis) axis_map.append(axis_name) axis_states[axis_name] = 0.0 # Get the button map. buf = array.array('H', [0] * 200) ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP for btn in buf[:num_buttons]: btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn) button_map.append(btn_name) button_states[btn_name] = 0 print('%d axes found: %s' % (num_axes, ', '.join(axis_map))) print('%d buttons found: %s' % (num_buttons, ', '.join(button_map))) # Spin child process to read the joystick # with blocking reads events = Queue() t = Process(target=readJoystick, args=(jsdev, events)) t.start() rclpy.init() node = Node("joystick_" + joyname) pubjoy = node.create_publisher(Joy, joyname + '/joystick') simTime = SimTimer(False, "veranda/timestamp", node) # Tick time at 10 hz dt = 0.1 def cb(): processJoystick(events, button_states, axis_states, button_map, axis_map) msg = formatJoystickMessage(button_states, axis_states) pubjoy.publish(msg) simTime.create_timer(dt, cb) rclpy.spin(node) node.destroy_node() rclpy.shutdown() t.terminate()
def main(): # INIT RCL ================================================================ rclpy.init() try: adpt.init_rclcpp() except RuntimeError: # Continue if it is already initialized pass # INIT GRAPH ============================================================== test_graph = graph.Graph() test_graph.add_waypoint(map_name, [0.0, -10.0]) # 0 test_graph.add_waypoint(map_name, [0.0, -5.0]) # 1 test_graph.add_waypoint(map_name, [5.0, -5.0]).set_holding_point(True) # 2 test_graph.add_waypoint(map_name, [-10.0, 0]) # 3 test_graph.add_waypoint(map_name, [-5.0, 0.0]) # 4 test_graph.add_waypoint(map_name, [0.0, 0.0]) # 5 test_graph.add_waypoint(map_name, [5.0, 0.0]) # 6 test_graph.add_waypoint(map_name, [10.0, 0.0]) # 7 test_graph.add_waypoint(map_name, [0.0, 5.0]) # 8 test_graph.add_waypoint(map_name, [5.0, 5.0]).set_holding_point(True) # 9 test_graph.add_waypoint(map_name, [0.0, 10.0]) # 10 assert test_graph.get_waypoint(2).holding_point assert test_graph.get_waypoint(9).holding_point assert not test_graph.get_waypoint(10).holding_point test_graph_legend = \ """ D : Dispenser I : Ingestor H : Holding Point K : Dock Numerals : Waypoints ---- : Lanes """ test_graph_vis = \ test_graph_legend + \ """ 10(I,K) | | 8------9(H) | | | | 3------4------5------6------7(D,K) | | | | 1------2(H) | | 0 """ test_graph.add_bidir_lane(0, 1) # 0 1 test_graph.add_bidir_lane(1, 2) # 2 3 test_graph.add_bidir_lane(1, 5) # 4 5 test_graph.add_bidir_lane(2, 6) # 6 7 test_graph.add_bidir_lane(3, 4) # 8 9 test_graph.add_bidir_lane(4, 5) # 10 11 test_graph.add_bidir_lane(5, 6) # 12 13 test_graph.add_dock_lane(6, 7, "A") # 14 15 test_graph.add_bidir_lane(5, 8) # 16 17 test_graph.add_bidir_lane(6, 9) # 18 19 test_graph.add_bidir_lane(8, 9) # 20 21 test_graph.add_dock_lane(8, 10, "B") # 22 23 assert test_graph.num_lanes == 24 test_graph.add_key(pickup_name, 7) test_graph.add_key(dropoff_name, 10) assert len(test_graph.keys) == 2 and pickup_name in test_graph.keys \ and dropoff_name in test_graph.keys # INIT FLEET ============================================================== profile = traits.Profile(geometry.make_final_convex_circle(1.0)) robot_traits = traits.VehicleTraits(linear=traits.Limits(0.7, 0.3), angular=traits.Limits(1.0, 0.45), profile=profile) # Manages delivery or loop requests adapter = adpt.MockAdapter("TestDeliveryAdapter") fleet = adapter.add_fleet(fleet_name, robot_traits, test_graph) # Set up task request callback function # we will only accept delivery task here def task_request_cb(task_profile): from rmf_task_msgs.msg import TaskType if (task_profile.description.task_type == TaskType.TYPE_DELIVERY): return True else: return False fleet.accept_task_requests(task_request_cb) # Set fleet battery profile battery_sys = battery.BatterySystem.make(24.0, 40.0, 8.8) mech_sys = battery.MechanicalSystem.make(70.0, 40.0, 0.22) motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys) ambient_power_sys = battery.PowerSystem.make(20.0) ambient_sink = battery.SimpleDevicePowerSink(battery_sys, ambient_power_sys) tool_power_sys = battery.PowerSystem.make(10.0) tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys) fleet.set_recharge_threshold(0.2) b_success = fleet.set_task_planner_params(battery_sys, motion_sink, ambient_sink, tool_sink) assert b_success, "set battery param failed" cmd_node = Node("RobotCommandHandle") # Test compute_plan_starts, which tries to place the robot on the navgraph # Your robot MUST be near a waypoint or lane for this to work though! starts = plan.compute_plan_starts(test_graph, map_name, [[-10.0], [0.0], [0.0]], adapter.now()) assert [x.waypoint for x in starts] == [3], [x.waypoint for x in starts] # Alternatively, if you DO know where your robot is, place it directly! starts = [plan.Start(adapter.now(), 0, 0.0)] # Lambda to insert an adapter def updater_inserter(handle_obj, updater): updater.update_battery_soc(1.0) handle_obj.updater = updater # Manages and executes robot commands robot_cmd = MockRobotCommand(cmd_node, test_graph) fleet.add_robot(robot_cmd, "T0", profile, starts, partial(updater_inserter, robot_cmd)) # INIT DISPENSERS ========================================================= dispenser = MockDispenser(dispenser_name) ingestor = MockIngestor(ingestor_name) # INIT TASK SUMMARY OBSERVER ============================================== # Note: this is used for assertation check on TaskSummary.Msg observer = TaskSummaryObserver() # FINAL PREP ============================================================== rclpy_executor = SingleThreadedExecutor() rclpy_executor.add_node(cmd_node) rclpy_executor.add_node(dispenser) rclpy_executor.add_node(ingestor) rclpy_executor.add_node(observer) # GO! ===================================================================== adapter.start() print("\n") print("# SENDING SINGLE DELIVERY REQUEST ################################") print(test_graph_vis) dispenser.reset() ingestor.reset() observer.reset() observer.add_task(test_name) task_desc = Type.CPPTaskDescriptionMsg() # this is the time when the robot reaches the pick up point task_desc.start_time_sec = int(time.time()) + 50 task_desc.delivery = Type.CPPDeliveryMsg(pickup_name, dispenser_name, dropoff_name, ingestor_name) task_profile = Type.CPPTaskProfileMsg() task_profile.description = task_desc task_profile.task_id = test_name adapter.dispatch_task(task_profile) rclpy_executor.spin_once(1) for i in range(1000): if observer.all_tasks_complete(): print("Tasks Complete.") break rclpy_executor.spin_once(1) # time.sleep(0.2) results = observer.count_successful_tasks() print("\n== DEBUG TASK REPORT ==") print("Visited waypoints:", robot_cmd.visited_waypoints) print(f"Sucessful Tasks: {results[0]} / {results[1]}") assert results[0] == results[1], "Not all tasks were completed." error_msg = "Robot did not take the expected route" assert robot_cmd.visited_waypoints == [ 0, 0, 5, 5, 6, 6, 7, 6, 5, 5, 8, 8, 10 ], error_msg # check if unstable partcipant works # this is helpful for to update the robot when it is print("Update a custom itinerary to fleet adapter") traj = schedule.make_trajectory(robot_traits, adapter.now(), [[3, 0, 0], [1, 1, 0], [2, 1, 0]]) route = schedule.Route("test_map", traj) participant = robot_cmd.updater.get_unstable_participant() routeid = participant.last_route_id participant.set_itinerary([route]) new_routeid = participant.last_route_id print(f"Previous route id: {routeid} , new route id: {new_routeid}") assert routeid != new_routeid # TODO(YL) There's an issue with during shutdown of the adapter, occurs # when set_itinerary() function above is used. Similarly with a non-mock # adpater, will need to address this in near future print("\n~ Shutting Down everything ~") cmd_node.destroy_node() observer.destroy_node() dispenser.destroy_node() ingestor.destroy_node() robot_cmd.stop() adapter.stop() rclpy_executor.shutdown() rclpy.shutdown()
def main(): # INIT RCL ================================================================ rclpy.init() adpt.init_rclcpp() # INIT GRAPH ============================================================== map_name = "test_map" test_graph = graph.Graph() test_graph.add_waypoint(map_name, [0.0, -10.0]) # 0 test_graph.add_waypoint(map_name, [0.0, -5.0]) # 1 test_graph.add_waypoint(map_name, [5.0, -5.0]).set_holding_point(True) # 2 test_graph.add_waypoint(map_name, [-10.0, 0]) # 3 test_graph.add_waypoint(map_name, [-5.0, 0.0]) # 4 test_graph.add_waypoint(map_name, [0.0, 0.0]) # 5 test_graph.add_waypoint(map_name, [5.0, 0.0]) # 6 test_graph.add_waypoint(map_name, [10.0, 0.0]) # 7 test_graph.add_waypoint(map_name, [0.0, 5.0]) # 8 test_graph.add_waypoint(map_name, [5.0, 5.0]).set_holding_point(True) # 9 test_graph.add_waypoint(map_name, [0.0, 10.0]) # 10 assert test_graph.get_waypoint(2).holding_point assert test_graph.get_waypoint(9).holding_point assert not test_graph.get_waypoint(10).holding_point """ 10(D) | | 8------9 | | | | 3------4------5------6------7(D) | | | | 1------2 | | 0 """ test_graph.add_bidir_lane(0, 1) # 0 1 test_graph.add_bidir_lane(1, 2) # 2 3 test_graph.add_bidir_lane(1, 5) # 4 5 test_graph.add_bidir_lane(2, 6) # 6 7 test_graph.add_bidir_lane(3, 4) # 8 9 test_graph.add_bidir_lane(4, 5) # 10 11 test_graph.add_bidir_lane(5, 6) # 12 13 test_graph.add_dock_lane(6, 7, "A") # 14 15 test_graph.add_bidir_lane(5, 8) # 16 17 test_graph.add_bidir_lane(6, 9) # 18 19 test_graph.add_bidir_lane(8, 9) # 20 21 test_graph.add_dock_lane(8, 10, "B") # 22 23 assert test_graph.num_lanes == 24 test_graph.add_key(pickup_name, 7) test_graph.add_key(dropoff_name, 10) assert len(test_graph.keys) == 2 and pickup_name in test_graph.keys \ and dropoff_name in test_graph.keys # INIT FLEET ============================================================== profile = traits.Profile(geometry.make_final_convex_circle(1.0)) robot_traits = traits.VehicleTraits(linear=traits.Limits(0.7, 0.3), angular=traits.Limits(1.0, 0.45), profile=profile) # Manages delivery or loop requests adapter = adpt.MockAdapter("TestDeliveryAdapter") fleet = adapter.add_fleet("test_fleet", robot_traits, test_graph) fleet.accept_delivery_requests(lambda x: True) cmd_node = Node("RobotCommandHandle") # Test compute_plan_starts, which tries to place the robot on the navgraph # Your robot MUST be near a waypoint or lane for this to work though! starts = plan.compute_plan_starts(test_graph, "test_map", [[-10.0], [0.0], [0.0]], adapter.now()) assert [x.waypoint for x in starts] == [3], [x.waypoint for x in starts] # Alternatively, if you DO know where your robot is, place it directly! starts = [plan.Start(adapter.now(), 0, 0.0)] # Lambda to insert an adapter def updater_inserter(handle_obj, updater): handle_obj.updater = updater # Manages and executes robot commands robot_cmd = MockRobotCommand(cmd_node, test_graph) fleet.add_robot(robot_cmd, "T0", profile, starts, partial(updater_inserter, robot_cmd)) # INIT TASK OBSERVER ====================================================== at_least_one_incomplete = False completed = False def task_cb(msg): nonlocal at_least_one_incomplete nonlocal completed if msg.state == TASK_STATE_COMPLETED: completed = True at_least_one_incomplete = False else: completed = False at_least_one_incomplete = True task_node = rclpy.create_node("task_summary_node") task_node.create_subscription(TaskSummary, "task_summaries", task_cb, 10) # INIT DISPENSERS ========================================================= quiet_dispenser = MockQuietDispenser(quiet_dispenser_name) flaky_dispenser = MockFlakyDispenser(flaky_dispenser_name) # FINAL PREP ============================================================== rclpy_executor = SingleThreadedExecutor() rclpy_executor.add_node(task_node) rclpy_executor.add_node(cmd_node) rclpy_executor.add_node(quiet_dispenser) rclpy_executor.add_node(flaky_dispenser) last_quiet_state = False last_flaky_state = False # GO! ===================================================================== adapter.start() print("# SENDING NEW REQUEST ############################################") request = adpt.type.CPPDeliveryMsg("test_delivery", pickup_name, quiet_dispenser_name, dropoff_name, flaky_dispenser_name) quiet_dispenser.reset() flaky_dispenser.reset() adapter.request_delivery(request) rclpy_executor.spin_once(1) for i in range(1000): if quiet_dispenser.success_flag != last_quiet_state: last_quiet_state = quiet_dispenser.success_flag print("== QUIET DISPENSER FLIPPED SUCCESS STATE ==", last_quiet_state) if flaky_dispenser.success_flag != last_flaky_state: last_flaky_state = flaky_dispenser.success_flag print("== FLAKY DISPENSER FLIPPED SUCCESS STATE ==", last_flaky_state) if quiet_dispenser.success_flag and flaky_dispenser.success_flag: rclpy_executor.spin_once(1) break rclpy_executor.spin_once(1) time.sleep(0.2) print("\n== DEBUG TASK REPORT ==") print("Visited waypoints:", robot_cmd.visited_waypoints) print("At least one incomplete:", at_least_one_incomplete) print("Completed:", completed) print() assert len(robot_cmd.visited_waypoints) == 6 assert all([x in robot_cmd.visited_waypoints for x in [0, 5, 6, 7, 8, 10]]) assert robot_cmd.visited_waypoints[0] == 2 assert robot_cmd.visited_waypoints[5] == 4 assert robot_cmd.visited_waypoints[6] == 3 assert robot_cmd.visited_waypoints[7] == 1 assert robot_cmd.visited_waypoints[8] == 2 assert robot_cmd.visited_waypoints[10] == 1 assert at_least_one_incomplete # Uncomment this to send a second request. # TODO(CH3): # But note! The TaskManager has to be fixed first to allow task pre-emption # print("# SENDING NEW REQUEST ##########################################") # request = adpt.type.CPPDeliveryMsg("test_delivery_two", # dropoff_name, # flaky_dispenser_name, # pickup_name, # quiet_dispenser_name) # quiet_dispenser.reset() # flaky_dispenser.reset() # adapter.request_delivery(request) # rclpy_executor.spin_once(1) # # for i in range(1000): # if quiet_dispenser.success_flag != last_quiet_state: # last_quiet_state = quiet_dispenser.success_flag # print("== QUIET DISPENSER FLIPPED SUCCESS STATE ==", # last_quiet_state) # # if flaky_dispenser.success_flag != last_flaky_state: # last_flaky_state = flaky_dispenser.success_flag # print("== FLAKY DISPENSER FLIPPED SUCCESS STATE ==", # last_flaky_state) # # if quiet_dispenser.success_flag and flaky_dispenser.success_flag: # rclpy_executor.spin_once(1) # break # # rclpy_executor.spin_once(1) # time.sleep(0.2) # # print("\n== DEBUG TASK REPORT ==") # print("Visited waypoints:", robot_cmd.visited_waypoints) # print("At least one incomplete:", at_least_one_incomplete) # print("Completed:", completed) # print() task_node.destroy_node() cmd_node.destroy_node() quiet_dispenser.destroy_node() flaky_dispenser.destroy_node() rclpy_executor.shutdown() rclpy.shutdown()