예제 #1
0
def replier(service_name, number_of_cycles, namespace):
    from test_msgs.service_fixtures import get_test_srv
    import rclpy

    service_pkg = 'test_msgs'
    module = importlib.import_module(service_pkg + '.srv')
    srv_mod = getattr(module, service_name)

    rclpy.init(args=[])

    node = rclpy.create_node('replier', namespace=namespace)

    srv_fixtures = get_test_srv(service_name)

    chatter_callback = functools.partial(
        replier_callback, srv_fixtures=srv_fixtures)

    node.create_service(
        srv_mod, 'test/service/' + service_name, chatter_callback)

    spin_count = 1
    print('replier: beginning loop')
    while rclpy.ok() and spin_count < number_of_cycles:
        rclpy.spin_once(node, timeout_sec=2)
        spin_count += 1
        print('spin_count: ' + str(spin_count))
    node.destroy_node()
    rclpy.shutdown()
예제 #2
0
def main(args=None):
    global logger
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_action_server')
    logger = node.get_logger()

    # Use a ReentrantCallbackGroup to enable processing multiple goals concurrently
    # Default goal callback accepts all goals
    # Default cancel callback rejects cancel requests
    action_server = ActionServer(
        node,
        Fibonacci,
        'fibonacci',
        execute_callback=execute_callback,
        cancel_callback=cancel_callback,
        callback_group=ReentrantCallbackGroup())

    # Use a MultiThreadedExecutor to enable processing goals concurrently
    executor = MultiThreadedExecutor()

    rclpy.spin(node, executor=executor)

    action_server.destroy()
    node.destroy_node()
    rclpy.shutdown()
예제 #3
0
def func_destroy_timers():
    import rclpy
    rclpy.init()

    node = rclpy.create_node('test_node3')

    timer1 = node.create_timer(0.1, None)
    timer2 = node.create_timer(1, None)
    timer2  # noqa

    assert 2 == len(node.timers)

    assert node.destroy_timer(timer1) is True

    assert 1 == len(node.timers)
    try:
        assert node.destroy_node() is True
    except SystemError:
        ret = False
    else:
        assert 0 == len(node.timers)
        assert node.handle is None
        ret = True
    finally:
        rclpy.shutdown()
        return ret
예제 #4
0
def main(args=None):
    rclpy.init(args)

    node = rclpy.create_node("minimal_client_async")

    cli = node.create_client(AddTwoInts, "add_two_ints")

    req = AddTwoInts.Request()
    req.a = 41
    req.b = 1
    # TODO(mikaelarguedas) remove this once wait for service implemented
    # wait for connection to be established
    # (no wait for service in Python yet)
    time.sleep(1)
    cli.call(req)
    while rclpy.ok():
        # TODO(mikaelarguedas) This is not the final API, and this does not scale
        # for multiple pending requests. This will change once an executor model is implemented
        # In the future the response will not be stored in cli.response
        if cli.response is not None:
            print("Result of add_two_ints: for %d + %d = %d" % (req.a, req.b, cli.response.sum))
            break
        rclpy.spin_once(node)

    rclpy.shutdown()
예제 #5
0
def talker(message_name, number_of_cycles, namespace):
    from test_msgs.message_fixtures import get_test_msg
    import rclpy

    message_pkg = 'test_msgs'
    module = importlib.import_module(message_pkg + '.msg')
    msg_mod = getattr(module, message_name)

    rclpy.init(args=[])

    node = rclpy.create_node('talker', namespace=namespace)

    chatter_pub = node.create_publisher(
        msg_mod, 'test/message/' + message_name)

    cycle_count = 0
    print('talker: beginning loop')
    msgs = get_test_msg(message_name)
    while rclpy.ok() and cycle_count < number_of_cycles:
        msg_count = 0
        for msg in msgs:
            chatter_pub.publish(msg)
            msg_count += 1
            print('publishing message #%d' % msg_count)
            time.sleep(0.01)
        cycle_count += 1
        time.sleep(0.1)
    node.destroy_node()
    rclpy.shutdown()
예제 #6
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_client_async')

    cli = node.create_client(AddTwoInts, 'add_two_ints')

    req = AddTwoInts.Request()
    req.a = 41
    req.b = 1
    while not cli.wait_for_service(timeout_sec=1.0):
        node.get_logger().info('service not available, waiting again...')

    future = cli.call_async(req)
    while rclpy.ok():
        rclpy.spin_once(node)
        if future.done():
            if future.result() is not None:
                node.get_logger().info(
                    'Result of add_two_ints: for %d + %d = %d' %
                    (req.a, req.b, future.result().sum))
            else:
                node.get_logger().info('Service call failed %r' % (future.exception(),))
            break

    node.destroy_node()
    rclpy.shutdown()
예제 #7
0
파일: client.py 프로젝트: esteve/examples
def main(args=None):
    rclpy.init(args=args)
    node = rclpy.create_node('minimal_client')
    cli = node.create_client(AddTwoInts, 'add_two_ints')

    req = AddTwoInts.Request()
    req.a = 41
    req.b = 1
    # TODO(mikaelarguedas) remove this once wait for service implemented
    # wait for connection to be established
    # (no wait for service in Python yet)
    time.sleep(1)

    cli.call(req)
    # when calling wait for future
    # spin should not be called in the main loop
    cli.wait_for_future()
    # TODO(mikaelarguedas) This is not the final API, and this does not scale
    # for multiple pending requests. This will change once an executor model is implemented
    # In the future the response will not be stored in cli.response
    print(
        'Result of add_two_ints: for %d + %d = %d' %
        (req.a, req.b, cli.response.sum))

    node.destroy_node()
    rclpy.shutdown()
예제 #8
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_publisher')
    publisher = node.create_publisher(String, 'topic', 10)

    msg = String()
    i = 0

    def timer_callback():
        nonlocal i
        msg.data = 'Hello World: %d' % i
        i += 1
        node.get_logger().info('Publishing: "%s"' % msg.data)
        publisher.publish(msg)

    timer_period = 0.5  # seconds
    timer = node.create_timer(timer_period, timer_callback)

    rclpy.spin(node)

    # Destroy the timer attached to the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_timer(timer)
    node.destroy_node()
    rclpy.shutdown()
예제 #9
0
def talker(message_name, rmw_implementation, number_of_cycles):
    import rclpy
    from rclpy.qos import qos_profile_default
    from rclpy.impl.rmw_implementation_tools import select_rmw_implementation
    from message_fixtures import get_test_msg

    message_pkg = 'test_communication'
    module = importlib.import_module(message_pkg + '.msg')
    msg_mod = getattr(module, message_name)

    select_rmw_implementation(rmw_implementation)

    rclpy.init([])

    node = rclpy.create_node('talker')

    chatter_pub = node.create_publisher(
        msg_mod, 'test_message_' + message_name, qos_profile_default)

    cycle_count = 0
    print('talker: beginning loop')
    msgs = get_test_msg(message_name)
    while rclpy.ok() and cycle_count < number_of_cycles:
        cycle_count += 1
        msg_count = 0
        for msg in msgs:
            chatter_pub.publish(msg)
            msg_count += 1
            print('publishing message #%d' % msg_count)
            time.sleep(0.1)
        time.sleep(1)
    rclpy.shutdown()
예제 #10
0
    def cancel_done(self, future):
        cancel_response = future.result()
        if len(cancel_response.goals_canceling) > 0:
            self.get_logger().info('Goal successfully canceled')
        else:
            self.get_logger().info('Goal failed to cancel')

        rclpy.shutdown()
예제 #11
0
def func_init_shutdown():
    import rclpy
    try:
        rclpy.init()
        rclpy.shutdown()
        return True
    except RuntimeError:
        return False
예제 #12
0
def main(args=None):
    rclpy.init(args=args)
    try:
        talker = ThrottledTalker()
        rclpy.spin(talker)
    finally:
        talker.destroy_node()
        rclpy.shutdown()
예제 #13
0
def main(args=None):
    rclpy.init(args=args)

    minimal_service = MinimalService()

    rclpy.spin(minimal_service)

    rclpy.shutdown()
예제 #14
0
def main(argv=sys.argv):
    rclpy.init(args=argv)

    node = rclpy.create_node(
        'initial_params_node', namespace='/', allow_undeclared_parameters=True)
    rclpy.spin(node)

    rclpy.shutdown()
    return 0
예제 #15
0
def func_double_shutdown():
    import rclpy
    rclpy.init()
    rclpy.shutdown()
    try:
        rclpy.shutdown()
    except RuntimeError:
        return True

    return False
예제 #16
0
파일: client.py 프로젝트: ros2/examples
    def get_result_callback(self, future):
        result = future.result().result
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Goal succeeded! Result: {0}'.format(result.sequence))
        else:
            self.get_logger().info('Goal failed with status: {0}'.format(status))

        # Shutdown after receiving a result
        rclpy.shutdown()
예제 #17
0
def main(args=None):
    rclpy.init(args)

    node = rclpy.create_node('add_two_ints_server')
    minimal_service = MinimalService(node)
    minimal_service  # prevent unused variable warning

    while rclpy.ok():
        rclpy.spin_once(node)

    rclpy.shutdown()
예제 #18
0
def main(args=None):
    rclpy.init(args=args)

    action_server = MinimalActionServer()

    # We use a MultiThreadedExecutor to handle incoming goal requests concurrently
    executor = MultiThreadedExecutor()
    rclpy.spin(action_server, executor=executor)

    action_server.destroy()
    rclpy.shutdown()
예제 #19
0
def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()
예제 #20
0
def func_destroy_node():
    import rclpy
    rclpy.init()
    node = rclpy.create_node('test_node1')
    ret = True
    try:
        node.destroy_node()
    except RuntimeError:
        ret = False
    finally:
        rclpy.shutdown()
        return ret
예제 #21
0
def main(args=None):
    rclpy.init(args=args)

    minimal_action_server = MinimalActionServer()

    # Use a MultiThreadedExecutor to enable processing goals concurrently
    executor = MultiThreadedExecutor()

    rclpy.spin(minimal_action_server, executor=executor)

    minimal_action_server.destroy()
    rclpy.shutdown()
예제 #22
0
def node_fixture(request):
    """Create a fixture with a node and helper executable."""
    rclpy.init()
    node = rclpy.create_node('tests_yaml', allow_undeclared_parameters=True)
    try:
        yield {
            'node': node,
            'executable': request.param,
        }
    finally:
        node.destroy_node()
        rclpy.shutdown()
예제 #23
0
def func_corrupt_node_handle():
    import rclpy
    rclpy.init()
    node = rclpy.create_node('test_node5')
    try:
        node.handle = 'garbage'
        ret = False
    except AttributeError:
        node.destroy_node()
        ret = True
    finally:
        rclpy.shutdown()
        return ret
예제 #24
0
def main(args=None):
    rclpy.init(args=args)
    try:
        executor = PriorityExecutor()
        executor.add_high_priority_node(Estopper())
        executor.add_node(Listener())
        executor.add_node(Talker())
        try:
            executor.spin()
        finally:
            executor.shutdown()
    finally:
        rclpy.shutdown()
예제 #25
0
def func_destroy_corrupted_node():
    import rclpy
    rclpy.init()
    node = rclpy.create_node('test_node2')
    assert node.destroy_node() is True
    node._handle = 'garbage'
    ret = False
    try:
        node.destroy_node()
    except SystemError:
        ret = True
    finally:
        rclpy.shutdown()
        return ret
예제 #26
0
def main(args=None):
    rclpy.init(args)

    node = rclpy.create_node('add_two_ints_server')

    srv = node.create_service(AddTwoInts, 'add_two_ints', add_two_ints_callback)
    while rclpy.ok():
        rclpy.spin_once(node)

    # Destroy the service attached to the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_service(srv)
    rclpy.shutdown()
예제 #27
0
def main(args=None):
    rclpy.init(args)

    node = rclpy.create_node('minimal_publisher')

    minimal_subscriber = MinimalSubscriber(node)
    minimal_subscriber  # prevent unused variable warning
    while rclpy.ok():
        rclpy.spin_once(node)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_node()
    rclpy.shutdown()
예제 #28
0
파일: listener.py 프로젝트: esteve/examples
def main(args=None):
    """
    Run a Listener node standalone.

    This function is called directly when using an entrypoint. Entrypoints are configured in
    setup.py. This along with the script installation in setup.cfg allows a listener node to be run
    with the command `ros2 run examples_rclpy_executors listener`.

    :param args: Arguments passed in from the command line.
    """
    rclpy.init(args=args)
    try:
        rclpy.spin(Listener())
    finally:
        rclpy.shutdown()
예제 #29
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_subscriber')

    subscription = node.create_subscription(
        String, 'topic', lambda msg: print('I heard: [%s]' % msg.data))
    subscription  # prevent unused variable warning

    rclpy.spin(node)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_node()
    rclpy.shutdown()
예제 #30
0
def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node('minimal_subscriber')

    subscription = node.create_subscription(String, 'topic', chatter_callback)
    subscription  # prevent unused variable warning

    while rclpy.ok():
        rclpy.spin_once(node)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    node.destroy_node()
    rclpy.shutdown()
예제 #31
0
def main(args=None):
    rclpy.init(args=args)
    laser_to_pointcloud = laserToPointcloud()
    rclpy.spin(laser_to_pointcloud)
    laser_to_pointcloud.destroy_node()
    rclpy.shutdown()
예제 #32
0
 def tearDownClass(cls):
     cls.node.destroy_node()
     rclpy.shutdown(context=cls.context)
예제 #33
0
def main(args=None):
    rclpy.init(args=args)
    node = getCameraDataNode()
    rclpy.spin(node)
    rclpy.shutdown()
def main(args=None):
    Bucket_list = []
    Cube_list = []

    rclpy.init(args=args)
    ic = image_converter()
    base = base_detection()
    navpublisher = navigation_publisher()
    classifier = image_classifier()

    # Hack to get two images which clears the ghost images
    while rclpy.ok() and ic.recievedImage is False:
        rclpy.spin_once(ic)

    ic.recievedImage = False
    while rclpy.ok() and ic.recievedImage is False:
        rclpy.spin_once(ic)

    # getbase here takes in raw image to draw contours on
    base.contourImg = ic.msg
    base.getBase(ic.msg)
    classifier.robot_ycord = base.robot_ycord

    # After drawing base contours pass it off to draw classifications
    classifier.contourImg = base.contourImg
    classifier.classify_objects(ic.msg)
    detection_array = classifier.detections
    cv2.imshow("detections and base", classifier.contourImg)
    cv2.waitKey(0)

    for obj in detection_array:
        # print("center Coordinates: ", obj.coordinateCenter)
        obj.coordinateCenter = base.get_world_coordinates(obj.coordinateCenter[0],obj.coordinateCenter[1])
        # print("world space coordinates: " , obj.coordinateCenter)

    for obj in detection_array:
        if isCube(obj) == False:
            new_bucket = Bucket(center_x = obj.coordinateCenter[0], 
                                center_y = obj.coordinateCenter[1],
                                classification= obj.classification)
            Bucket_list.append(new_bucket)
        else:
            new_Cube = Cube(center_x = obj.coordinateCenter[0],
                            center_y = obj.coordinateCenter[1],
                            classification= obj.classification,
                            angle= obj.angle)
            Cube_list.append(new_Cube)
    
    print("start read environment") 
    #take in cubelist, bucketlist 
    print("start executeCommand")

    # for obj in Cube_list:
    #     print("class: ", obj.classification)
    #     print("X: ", obj.center_x)
    #     print("Y: ", obj.center_y)
    
    o_x = -3.139559138676179
    o_y = -0.015483082006265326

    first_run = True
    for cube in Cube_list:
        for bucket in Bucket_list:
            if colorCheck(cube,bucket) == True:
                #start navigation
                print("moving cube", cube.classification, "into bucket", bucket.classification)
                # If the angle is negative make it positive
                if cube.angle < 0:
                    cube.angle * -1.0
                o_z = (numpy.pi/2 - cube.angle) - numpy.pi/4 


                #initialize coordinates
                cube.center_x = cube.center_x * -1.0 
                cube.center_y = cube.center_y * -1.0

                bucket.center_x = bucket.center_x * -1.0
                bucket.center_y = bucket.center_y * -1.0
            
                #cube hover 
                print("Hover over cube position\n")
                p_x = cube.center_x
                p_y = cube.center_y
                p_z = 1.10756253284
                print("Going to", p_x, p_y)
                if first_run:
                    navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 62.0, "open")
                    first_run = False
                else:
                    navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 65.0, "open")

                # cube position (grab)
                print("Lowering position\n")
                p_x = cube.center_x
                p_y = cube.center_y
                p_z = 1.040006
                navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 65.0, "close")
      
                # cube position (post-grab)
                print("Rise up\n")
                p_x = cube.center_x
                p_y = cube.center_y
                p_z = 1.20756253284
                navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 65.0)

                # bucket position hover
                print("Hover over bucket\n")
                p_x = bucket.center_x
                p_y = bucket.center_y
                p_z = 1.20756253284
                navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 65.0, "open")

    
    # Return home
    navpublisher.publish_coordinates(0.0, 0.0, 2.1, 0.0, 0.0, 0.0, 62.0)
    rclpy.shutdown()
def main(args=None):
    rclpy.init(args=args)
    node = NumberPublisherNode()
    rclpy.spin(node)
    rclpy.shutdown()
	def callback_kill(self, msg):
		del self 
		self.destroy_node()
		rclpy.shutdown()
예제 #37
0
def main(args=None):
    rclpy.init(args=args)
    node = my_node()
    rclpy.spin(node)
    rclpy.shutdown()
예제 #38
0
def main(args=None):
    rclpy.init(args=args)
    node = rclpy.create_node("my_first_node")
    print("Started the node")
    rclpy.spin(node)
    rclpy.shutdown()
예제 #39
0
def main(args=None):
    rclpy.init(args=args)
    node = TTSNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
예제 #40
0
파일: direct.py 프로젝트: ruffsl/ros2cli
 def __exit__(self, exc_type, exc_value, traceback):
     self.node.destroy_node()
     rclpy.shutdown()
예제 #41
0
파일: ex.py 프로젝트: khaledsayed696/Demo_1
def main (args=None):
    rclpy.init(args=args)
    node1=my_server()
    rclpy.spin(node1)
    rclpy.shutdown()
    def destroy(self):
        """
        Shuts down ROS node

        """
        rclpy.shutdown()
예제 #43
0
def main(args=None):
    rclpy.init(args=args)
    node1 = stopAtWallNode()
    rclpy.spin(node1)
    rclpy.shutdown()
def main():
    """
    Entry point for the tree watcher script.
    """
    ####################
    # Arg Parsing
    ####################

    # command_line_args = rclpy.utilities.remove_ros_args(command_line_args)[1:]
    command_line_args = None
    parser = command_line_argument_parser(formatted_for_sphinx=False)
    args = parser.parse_args(command_line_args)

    # mode is None if the user didn't specify any option in the exclusive group
    if args.mode is None:
        args.mode = py_trees_ros.trees.WatcherMode.SNAPSHOTS
    args.snapshot_period = 2.0 if (args.statistics or args.blackboard_data or args.blackboard_activity) else py_trees.common.Duration.INFINITE.value
    tree_watcher = py_trees_ros.trees.Watcher(
        namespace_hint=args.namespace_hint,
        topic_name=args.topic_name,
        parameters=py_trees_ros.trees.SnapshotStream.Parameters(
            blackboard_data=args.blackboard_data,
            blackboard_activity=args.blackboard_activity,
            snapshot_period=args.snapshot_period
        ),
        mode=args.mode,
        statistics=args.statistics,
    )

    ####################
    # Setup
    ####################
    rclpy.init(args=None)
    try:
        tree_watcher.setup(timeout_sec=5.0)
    # setup discovery fails
    except py_trees_ros.exceptions.NotFoundError as e:
        print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset)
        sys.exit(1)
    # setup discovery finds duplicates
    except py_trees_ros.exceptions.MultipleFoundError as e:
        print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset)
        if args.namespace is None:
            print(console.red + "\nERROR: select one with the --namespace argument\n" + console.reset)
        else:
            print(console.red + "\nERROR: but none matching the requested '{}'\n".format(args.namespace) + console.reset)
        sys.exit(1)
    except py_trees_ros.exceptions.TimedOutError as e:
        print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset)
        sys.exit(1)

    ####################
    # Execute
    ####################
    executor = rclpy.executors.SingleThreadedExecutor()
    executor.add_node(node=tree_watcher.node)
    try:
        while True:
            if not rclpy.ok():
                break
            if tree_watcher.done:
                if tree_watcher.xdot_process is None:
                    # no xdot found on the system, just break out and finish
                    break
                elif tree_watcher.xdot_process.poll() is not None:
                    # xdot running, wait for it to terminate
                    break
            executor.spin_once(timeout_sec=0.1)
    except KeyboardInterrupt:
        pass
    finally:
        if tree_watcher.xdot_process is not None:
            if tree_watcher.xdot_process.poll() is not None:
                tree_watcher.xdot_process.terminate()
        tree_watcher.shutdown()
        rclpy.shutdown()
def main(args=None):
    rclpy.init(args=args)
    node = RobotNewsStationNode()
    rclpy.spin(node)
    rclpy.shutdown()
예제 #46
0
def main(args=None):
    rclpy.init(args=args)
    fire_alarm = fire_alarm_sound()
    rclpy.spin(fire_alarm)
    fire_alarm.destroy_node()
    rclpy.shutdown()
예제 #47
0
 def shutdown(self):
     self._executor.shutdown()
     rclpy.shutdown(context=self._context)
     self.destroy_node()
     self._thread.join()
예제 #48
0
 def executor(self):
     while rclpy.ok():
         rclpy.spin_once(self.kuka_node)
     self.kuka_node.destroy_node()
     rclpy.shutdown()
예제 #49
0
def main():
    rclpy.init()
    odom = WheelOdom()
    rclpy.spin(odom)
    rclpy.shutdown()
예제 #50
0
    def socket(self, ip, port):
        self.UDPsocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        # Bind the socket to the port
        server_address = (ip, port)

        #TEST:
        local_port = 20001
        local_hostname = socket.gethostname()
        local_ip = socket.gethostbyname(local_hostname)
        server_address = (local_ip, local_port)

        os.system('clear')
        print(cl_pink('==========================================\n'))

        print(cl_cyan('Starting up on:'), 'IP:', local_ip, 'Port:', local_port)
        try:
            self.UDPsocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
            self.UDPsocket.bind(server_address)
        except:
            print(cl_red('Error: ') + "Connection for KUKA cannot assign requested address:", ip, port)
            os._exit(-1)

        # Wait for a connection
        print(cl_cyan('Waiting for a connection...'))

        data, self.client_address = self.UDPsocket.recvfrom(self.BUFFER_SIZE)
        print(cl_cyan('Connection from: '), self.client_address)
        print(cl_cyan('Message: '), data)


        self.UDPsocket.sendto("HALLA".encode(), self.client_address)
        print("Responded KUKA")

        self.isconnected = True
        last_read_time = time.time()
        while self.isconnected:
            try:
                data,addr = self.UDPsocket.recvfrom(self.BUFFER_SIZE)
                ######  TEST DECODE #######
                #data.decode("utf-8")
                data = data.decode()
                ##########################
                last_read_time = time.time()  # Keep received time
                # Process the received data package
                for pack in data.split(">"):  # parsing data pack
                    cmd_splt = pack.split()
                    if len(pack) and cmd_splt[0] == 'Joint_Pos':  # If it's JointPosition
                        tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
                        if len(tmp) == 7: self.JointPosition = (tmp, last_read_time)

                    if len(pack) and cmd_splt[0] == 'Tool_Pos':  # If it's ToolPosition
                        tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
                        if len(tmp) == 6: self.ToolPosition = (tmp, last_read_time)

                    if len(pack) and cmd_splt[0] == 'Tool_Force':  # If it's ToolForce
                        tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
                        if len(tmp) == 3:
                            self.ToolForce = (tmp, last_read_time)

                    if len(pack) and cmd_splt[0] == 'Tool_Torque':  # If it's ToolTorque
                        tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
                        if len(tmp) == 3:
                            self.ToolTorque = (tmp, last_read_time)

                    if len(pack) and cmd_splt[0] == 'isCompliance':  # If isCompliance
                        if cmd_splt[1] == "false":
                            self.isCompliance = (False, last_read_time)
                        elif cmd_splt[1] == "true":
                            self.isCompliance = (True, last_read_time)

                    if len(pack) and cmd_splt[0] == 'isCollision':  # If isCollision
                        if cmd_splt[1] == "false":
                            self.isCollision = (False, last_read_time)
                        elif cmd_splt[1] == "true":
                            self.isCollision = (True, last_read_time)

                    if len(pack) and cmd_splt[0] == 'isReadyToMove':  # If isReadyToMove
                        if cmd_splt[1] == "false":
                            self.isReadyToMove = (False, last_read_time)
                        elif cmd_splt[1] == "true":
                            self.isReadyToMove = (True, last_read_time)

                    if len(pack) and cmd_splt[0] == 'isMastered':  # If isMastered
                        if cmd_splt[1] == "false":
                            self.isMastered = (False, last_read_time)
                        elif cmd_splt[1] == "true":
                            self.isMastered = (True, last_read_time)

                    if len(pack) and cmd_splt[0] == 'OperationMode':  # If OperationMode
                        self.OperationMode = (cmd_splt[1], last_read_time)

                    if len(pack) and cmd_splt[0] == 'JointAcceleration':  # If it's JointAcceleration
                        self.JointAcceleration = (float(cmd_splt[1]), last_read_time)

                    if len(pack) and cmd_splt[0] == 'JointVelocity':  # If it's JointVelocity
                        self.JointVelocity = (float(cmd_splt[1]), last_read_time)

                    if len(pack) and cmd_splt[0] == 'JointJerk':  # If it's JointJerk
                        self.JointJerk = (float(cmd_splt[1]), last_read_time)

                    if len(pack) and cmd_splt[0] == 'isFinished':  # If isFinished
                        if cmd_splt[1] == "false":
                            self.isFinished = (False, last_read_time)
                        elif cmd_splt[1] == "true":
                            self.isFinished = (True, last_read_time)
                    if len(pack) and cmd_splt[0] == 'hasError':  # If hasError
                        if cmd_splt[1] == "false":
                            self.hasError = (False, last_read_time)
                        elif cmd_splt[1] == "true":
                            self.hasError = (True, last_read_time)

                    if (all(item != None for item in self.JointPosition[0]) and
                            all(item != None for item in self.ToolPosition[0]) and
                            all(item != None for item in self.ToolForce[0]) and
                            all(item != None for item in self.ToolTorque[0]) and
                            self.isCompliance[1] != None and
                            self.isCollision[1] != None and
                            self.isReadyToMove[1] != None and
                            self.isMastered[1] != None and
                            self.OperationMode[1] != None):
                        # self.OprationMode = (None, None)
                        self.isready = True
                    else:
                        self.isready = False
            except:
                elapsed_time = time.time() - last_read_time
                if elapsed_time > 5.0:  # Didn't receive a pack in 5s
                    print("exception!!")
                    self.close()  # Disconnect from iiwa
                    self.isconnected = False
                    print(cl_lightred('No packet received from iiwa for 5s!'))

        print("SHUTTING DOWN")
        #self.connection.shutdown(socket.SHUT_RDWR)
        #self.connection.close()
        self.UDPsocket.close()
        self.isconnected = False
        print(cl_lightred('Connection is closed!'))
        # NOE JEG SLANG P SELV :`)
        rclpy.shutdown()
예제 #51
0
def main(args: Any = None) -> None:
    rclpy.init(args=args)
    thymio_driver = ThymioDriver(namespace='', standalone=True)
    rclpy.spin(thymio_driver)
    thymio_driver.destroy_node()
    rclpy.shutdown()
예제 #52
0
 def tearDown(self):
     self.node.destroy_node()
     self.executor.shutdown()
     rclpy.shutdown(context=self.context)
def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument(
        "--max-number-of-actions",
        "-a",
        type=int,
        required=True,
        help="""Maximum numbers of actions that are processed.  After this the
            backend shuts down automatically.
        """,
    )
    camera_group = parser.add_mutually_exclusive_group()
    camera_group.add_argument(
        "--cameras",
        "-c",
        action="store_true",
        help="Run camera backend.",
    )
    camera_group.add_argument(
        "--cameras-with-tracker",
        action="store_true",
        help="Run camera backend with integrated object tracker.",
    )
    parser.add_argument(
        "--robot-logfile",
        type=str,
        help="""Path to a file to which the robot data log is written.  If not
            specified, no log is generated.
        """,
    )
    parser.add_argument(
        "--camera-logfile",
        type=str,
        help="""Path to a file to which the camera data is written.  If not
            specified, no log is generated.
        """,
    )
    args = parser.parse_args()

    rclpy.init()
    node = NotificationNode("trifinger_data")

    logger = node.get_logger()

    cameras_enabled = False
    if args.cameras:
        cameras_enabled = True
        from trifinger_cameras import tricamera
    elif args.cameras_with_tracker:
        cameras_enabled = True
        import trifinger_object_tracking.py_tricamera_types as tricamera

    if cameras_enabled:
        logger.info("Start camera data")

        camera_data = tricamera.MultiProcessData("tricamera", True,
                                                 CAMERA_TIME_SERIES_LENGTH)

    logger.info("Start robot data")

    # Storage for all observations, actions, etc.
    robot_data = robot_interfaces.trifinger.MultiProcessData(
        "trifinger", True, history_size=ROBOT_TIME_SERIES_LENGTH)

    if args.robot_logfile:
        robot_logger = robot_interfaces.trifinger.Logger(
            robot_data, buffer_limit=args.max_number_of_actions)
        robot_logger.start()

    if cameras_enabled and args.camera_logfile:
        camera_fps = 10
        robot_rate_hz = 1000
        # make the logger buffer a bit bigger as needed to be on the safe side
        buffer_length_factor = 1.5

        episode_length_s = args.max_number_of_actions / robot_rate_hz
        # Compute camera log size based on number of robot actions plus some
        # buffer
        log_size = int(camera_fps * episode_length_s * buffer_length_factor)

        logger.info("Initialize camera logger with buffer size %d" % log_size)
        camera_logger = tricamera.Logger(camera_data, log_size)

    logger.info("Data backend is ready")

    # send ready signal
    node.publish_status("READY")

    if cameras_enabled and args.camera_logfile:
        # wait for first action to be sent by the user (but make sure to not
        # block when shutdown is requested)
        while (not robot_data.desired_action.wait_for_timeindex(
                0, max_duration_s=1) and not node.shutdown_requested):
            rclpy.spin_once(node, timeout_sec=0)

        camera_logger.start()
        logger.info("Start camera logging")

    while not node.shutdown_requested:
        rclpy.spin_once(node)

    logger.debug("Received shutdown signal")

    if cameras_enabled and args.camera_logfile:
        logger.info("Save recorded camera data to file %s" %
                    args.camera_logfile)
        camera_logger.stop_and_save(args.camera_logfile)

    if args.robot_logfile:
        logger.info("Save robot data to file %s" % args.robot_logfile)
        robot_logger.stop_and_save(
            args.robot_logfile,
            robot_interfaces.trifinger.Logger.Format.BINARY)

    rclpy.shutdown()
    return 0
 def get_result_callback(self, future):
     result = future.result().result
     self.get_logger().warn(f'Action Done !! Result: {result.sequence}')
     rclpy.shutdown()
def main():
    rclpy.init()

    sim_time_param = is_sim_time()

    node = rclpy.create_node(
        'set_thrusters_output_efficiency',
        allow_undeclared_parameters=True, 
        automatically_declare_parameters_from_overrides=True,
        parameter_overrides=[sim_time_param])

    node.get_logger().info('Set the thruster output efficiency for vehicle, namespace=' + self.get_namespace())
    
    # sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
    # node.set_parameters([sim_time])

    starting_time = 0.0
    if node.has_parameter('starting_time'):
        starting_time = float(node.get_parameter('starting_time').value)

    node.get_logger().info('Starting time= {} s'.format(starting_time))

    duration = 0.0
    if node.has_parameter('duration'):
        duration = float(node.get_parameter('duration').value)

    if duration <= 0.0:
        raise RuntimeError('Duration not set, leaving node...')

    node.get_logger().info('Duration [s]=', ('Inf.' if duration < 0 else str(duration)))

    if node.has_parameter('efficiency'):
        efficiency = float(node.get_parameter('efficiency').value)
        if efficiency < 0 or efficiency > 1:
            raise RuntimeError('Invalid thruster output efficiency, leaving node...')
    else:
        raise RuntimeError('Thruster output efficiency not set, leaving node...')

    thruster_id = -1
    if node.has_parameter('thruster_id'):
        if node.get_parameter('thruster_id').type_ == rclpy.Parameter.Type.INTEGER:
            thruster_id = node.get_parameter('thruster_id').get_parameter_value().integer_value
    else:
        raise RuntimeError('Thruster ID not given')

    if thruster_id < 0:
        raise RuntimeError('Invalid thruster ID')

    node.get_logger().info('Setting thruster output efficiency #{} to {}'.format(thruster_id, 100 * efficiency))

    vehicle_name = node.get_namespace().replace('/', '')

    srv_name = build_service_name(vehicle_name, thruster_id, 'set_thrust_force_efficiency')
   
    set_eff = node.create_client(SetThrusterEfficiency, srv_name)
        
    if not set_eff.wait_for_service(timeout_sec=2):
        raise RuntimeError('Service %s not available! Closing node...' %(srv_name))

    FREQ = 100
    rate = node.create_rate(FREQ)
    thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
    thread.start()
    while time_in_float_sec(node.get_clock().now()) < starting_time:
        #Just a guard for really short timeouts
        if 1.0 / FREQ < starting_time: 
            rate.sleep()

    # rate = node.create_rate(100)
    # while time_in_float_sec(node.get_clock().now()) < starting_time:
    #     rate.sleep()

    req = SetThrusterEfficiency.Request()
    req.efficiency = efficiency
    #success = set_eff.call(efficiency)

    future = set_eff.call_async(efficiency)

    #NB : spining is done from another thread    
    while not future.done():
        try:
            response = future.result()
        except Exception as e:
            node.get_logger().info('Service call ' + srv_name + ' failed, error=' + str(e)):
        else:
            node.get_logger().info('Time={} s'.format(rtime_in_float_sec(node.get_clock().now())))
            node.get_logger().info('Current thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))

    if duration > 0:
        while time_in_float_sec(node.get_clock().now()) < starting_time + duration:
            if 1.0 / FREQ < starting_time + duration:
                rate.sleep()

        # rate = node.create_rate.Rate(100)
        # while time_in_float_sec(node.get_clock().now()) < starting_time + duration:
        #     rate.sleep()

        req.efficiency = 1.0
        success = set_eff.call(efficiency)

        while not future.done():
            try:
                response = future.result()
            except Exception as e:
                node.get_logger().info('Service call ' + srv_name + ' failed, error=' + str(e)):
            else:
                node.get_logger().info('Time={} s'.format(time_in_float_sec(node.get_clock().now())))
                node.get_logger().infoint(
                    'Returning to previous thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))

        # if success:
        #     print('Time={} s'.format(time_in_float_sec(node.get_clock().now())))
        #     print('Returning to previous thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))

    node.get_logger().info('Leaving node...')

    node.destroy_node()
    rclpy.shutdown()
    thread.join()
예제 #56
0
 def tearDownClass(cls):
     cls.node.destroy_node()
     rclpy.shutdown()
예제 #57
0
파일: key_teleop.py 프로젝트: pgold/roompi
def main(stdscr):
    rclpy.init()
    node = RoompiKeyTeleop(MainWindow(stdscr))
    rclpy.spin(node)
    rclpy.shutdown()
예제 #58
0
def main(args=None):
    rclpy.init(args=args)
    command_node = CVCommandsNode()
    rclpy.spin(command_node)
    rclpy.shutdown()
def teardown_module(module):
    console.banner("ROS Shutdown")
    rclpy.shutdown()
예제 #60
0
                # アクションのキャンセル
                goal_handle.set_canceled()
                self.get_logger().info('goal canceled')
                return Fibonacci.Result()

            # フィボナッチ数列の更新
            msg.sequence.append(msg.sequence[i] + msg.sequence[i - 1])
            self.get_logger().info('feedback: {0}'.format(msg.sequence))
            # アクションのフィードバックの送信
            goal_handle.publish_feedback(msg)
            # 1秒待機(重たい処理の代わり)
            time.sleep(1)

        # アクションの実行結果の送信
        goal_handle.set_succeeded()
        result = Fibonacci.Result()
        result.sequence = msg.sequence
        self.get_logger().info('result: {0}'.format(result.sequence))
        return result


if __name__ == '__main__':
    rclpy.init(args=args)
    minimal_action_server = MinimalActionServer()
    # マルチスレッドでminimal_action_serverノードを実行し、
    # 複数のアクションクライアントを同時処理
    executor = MultiThreadedExecutor()
    rclpy.spin(minimal_action_server, executor=executor)
    minimal_action_server.destroy()
    rclpy.shutdown()