def __init__(self, name): Node.__init__(self, name) self.declare_parameter( 'param_not_set', descriptor=ParameterDescriptor(description='not set')) self.declare_parameter( 'param_bool', True, ParameterDescriptor(description='boolean', read_only=True)) self.declare_parameter( 'param_int', 1234, ParameterDescriptor(integer_range=[ IntegerRange(from_value=-1000, to_value=10000, step=2) ])) self.declare_parameter('param_double', 3.14) self.declare_parameter('param_string', 'foobar') self.declare_parameter('param_bool_array', [True, False]) self.declare_parameter('param_int_array', [1, 2, 3]) self.declare_parameter( 'param_double_array', [1.50000, 23.50120, 123.0010], ParameterDescriptor(floating_point_range=[ FloatingPointRange( from_value=-1000.5, to_value=1000.5, step=0.0001) ])) self.declare_parameter('param_string_array', ['foo', 'bar']) self.declare_parameter('param_byte_array', [b'\x01', b'\x02', b'\x03', b'\x04'])
def __init__(self, parent_frame_id: str = "world", child_frame_id: str = "unknown_child_id", use_sim_time: bool = True, node_name: str = 'drl_grasping_camera_tf_broadcaster'): try: rclpy.init() except: if not rclpy.ok(): import sys sys.exit("ROS 2 could not be initialised") Node.__init__(self, node_name) self.set_parameters([ Parameter('use_sim_time', type_=Parameter.Type.BOOL, value=use_sim_time) ]) qos = QoSProfile(durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.KEEP_ALL) self._tf2_broadcaster = StaticTransformBroadcaster(self, qos=qos) self._transform_stamped = TransformStamped() self.set_parent_frame_id(parent_frame_id) self.set_child_frame_id(child_frame_id)
def __init__(self): Node.__init__(self, "sp_ui") Callbacks.__init__(self) Callbacks.trigger_node = self.trigger self.subscriber = self.create_subscription(String, "/sp/state_flat", self.sp_cmd_callback, 10) self.state_publisher = self.create_client(sp_msgs.srv.Json, "/sp/set_state") self.get_logger().info("Sequence Planner UI, up and running")
def __init__(self): # Initialise node try: rclpy.init() except: if not rclpy.ok(): import sys sys.exit("ROS 2 could not be initialised") Node.__init__(self, "octree_creator_test") self.__point_cloud_sub = self.create_subscription( PointCloud2, "rgbd_camera/points", self.point_cloud_callback, 1) self.octree_creator = OctreeCreator()
def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs): """ Create a lifecycle node. See rclpy.lifecycle.LifecycleNodeMixin.__init__() and rclpy.node.Node() for the documentation of each parameter. """ Node.__init__(self, node_name, **kwargs) LifecycleNodeMixin.__init__( self, enable_communication_interface=enable_communication_interface)
def __init__(self, name, planning_rate: float, planner: AbstractPlannerImplementation): self.name = name Node.__init__(self, "planner_" + name) # Abstract implementation of planner (adapter) self.planner_adapter = planner # Basic I/O interface (input and refined trajectory) # Subscriber interface for trajectory planning self._refined_trajectory = self.create_publisher( Trajectory, '/'.join([self.name, 'refined_trajectory']), 1) self._input_trajectory = None # Planner timer cycle self._plan_timer = self.create_timer(planning_rate, self.cb_timer_planner) self.get_logger().info("Starting planner node {0}".format(self.name))
def __init__(self, server_name, state, path): """Traverse the smach tree starting at root, and construct introspection proxies for getting and setting debug state.""" Node.__init__(self, server_name) # A list of introspection proxies self._proxies = [] # Store args self._server_name = server_name self._state = state self._path = path self._executor = SingleThreadedExecutor() self._executor.add_node(self) self._spinner = threading.Thread(target=self._executor.spin) self._spinner.start()
def __init__(self): Node.__init__(self, "pgcd_comp", allow_undeclared_parameters = True, automatically_declare_parameters_from_overrides = True) Interpreter.__init__(self, self.get_name()) TFUpdater.__init__(self) # program self.id = self.get_name() self.prog_path = self.get_parameter('program_location')._value + self.id + '.rosl' rclpy.logging._root_logger.log("PGCD creating " + self.id + " running " + self.prog_path, LoggingSeverity.INFO) # hardware module = importlib.import_module(self.get_parameter('object_module_name')._value) class_ = getattr(module, self.get_parameter('object_class_name')._value) self.robot = class_() # runtime self.tfBuffer = tf2_ros.Buffer() self.tf2_listener = tf2_ros.TransformListener(self.tfBuffer, self) self.tf2_setup(self.robot)
def __init__(self, topic: str, is_point_cloud: bool, use_sim_time: bool = True, node_name: str = 'drl_grasping_camera_sub'): 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) ]) # Prepare the subscriber if is_point_cloud: observation_type = PointCloud2 else: observation_type = Image self.__observation = observation_type() self.__observation_sub = self.create_subscription( msg_type=observation_type, topic=topic, callback=self.observation_callback, qos_profile=QoSProfile( durability=QoSDurabilityPolicy.SYSTEM_DEFAULT, reliability=QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.SYSTEM_DEFAULT)) self.__observation_mutex = Lock() self.__new_observation_available = False # Spin the node in a separate thread self._executor = SingleThreadedExecutor() self._executor.add_node(self) self._executor_thread = Thread(target=self._executor.spin, args=(), daemon=True) self._executor_thread.start()
def __init__(self) -> None: """ Class constructor for path planning node Args: Returns: """ # --------------------------------------------------------------------- Node.__init__(self, node_name="planner_node") # Allow callbacks to be executed in parallel without restriction. self.callback_group = ReentrantCallbackGroup() # --------------------------------------------------------------------- # Environment variables for forware and turn profiles self._TURN_ACELERATION_FC = float( os.getenv("TURN_ACELERATION_FC", default=0.3)) self._TURN_CRTL_POINTS = float( os.getenv("TURN_CRTL_POINTS", default=30)) self._FORWARE_ACELERATION_FC = float( os.getenv("FORWARE_ACELERATION_FC", default=0.3)) self._FORWARE_CRTL_POINTS = float( os.getenv("FORWARE_CRTL_POINTS", default=30)) self._TURN_TIME = float(os.getenv("TURN_TIME", default=3.0)) # --------------------------------------------------------------------- # Map features self.map_points = [] # Landmarks or keypoints in map self.map_duration = 0.0 # Map duration in [s] self.map_difficulty = 0.0 # Map difficulty [0.0-5.0] self.map_distance = 0.0 # Map distance in [m] self.way_points = {} # List of waypoints in the path planning routine self._in_execution = False # Read routines from the yaml file in the configs folder self.routines = read_yaml_file( CONF_PATH="/workspace/planner/configs", FILE_NAME="routines.yaml", ) # --------------------------------------------------------------------- # Subscribers self.sub_start_routine = self.create_subscription( msg_type=Int32, topic="/graphics/start_routine", callback=self.cb_start_routine, qos_profile=qos_profile_sensor_data, callback_group=self.callback_group, ) self.kiwibot_state = Kiwibot() self.sub_kiwibot_stat = self.create_subscription( msg_type=Kiwibot, topic="/kiwibot/status", callback=self.cb_kiwibot_status, qos_profile=qos_profile_sensor_data, callback_group=self.callback_group, ) # --------------------------------------------------------------------- # Publishers self.pub_path_planner = self.create_publisher( msg_type=planner_msg, topic="/path_planner/msg", qos_profile=qos_profile_sensor_data, callback_group=self.callback_group, ) self.pub_speaker = self.create_publisher( msg_type=Int8, topic="/device/speaker/command", qos_profile=qos_profile_sensor_data, callback_group=self.callback_group, ) # --------------------------------------------------------------------- # Services # service client to turn the robot self.cli_robot_turn = self.create_client(Turn, "/robot/turn") # service client to move the robot self.cli_robot_move = self.create_client(Move, "/robot/move") try: self.robot_turn_req = Turn.Request() self.robot_move_req = Move.Request() except Exception as e: printlog( msg="No services for robot actions, {}".format(e), msg_type="ERROR", )
def __init__(self): Node.__init__("goal_tester_node") self._pub_goal = self.create_publisher(Trajectory, "initial_goal")
def __init__(self, robot_frame_id: str = "panda_link0", min_bound: Tuple[float, float, float] = (-1.0, -1.0, -1.0), max_bound: Tuple[float, float, float] = (1.0, 1.0, 1.0), normals_radius: float = 0.05, normals_max_nn: int = 10, include_color: bool = False, depth: int = 4, full_depth: int = 2, node_dis: bool = True, node_feature: bool = False, split_label: bool = False, adaptive: bool = False, adp_depth: int = 4, th_normal: float = 0.1, th_distance: float = 2.0, extrapolate: bool = False, save_pts: bool = False, key2xyz: bool = False, use_sim_time: bool = True, debug_draw: bool = False, debug_write_octree: bool = False, node_name: str = 'drl_grasping_octree_creator'): # Initialise node 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) ]) # Create tf2 buffer and listener for transform lookup self.__tf2_buffer = tf2_ros.Buffer() self.__tf2_listener = tf2_ros.TransformListener( buffer=self.__tf2_buffer, node=self) # Parameters self._robot_frame_id = robot_frame_id self._min_bound = min_bound self._max_bound = max_bound self._normals_radius = normals_radius self._normals_max_nn = normals_max_nn self._include_color = include_color self._depth = depth self._full_depth = full_depth self._debug_draw = debug_draw self._debug_write_octree = debug_write_octree # Create a converter between points and octree self._points_to_octree = ocnn.Points2Octree(depth=depth, full_depth=full_depth, node_dis=node_dis, node_feature=node_feature, split_label=split_label, adaptive=adaptive, adp_depth=adp_depth, th_normal=th_normal, th_distance=th_distance, extrapolate=extrapolate, save_pts=save_pts, key2xyz=key2xyz, bb_min=min_bound, bb_max=max_bound) # Spin executor in another thread self._executor = MultiThreadedExecutor(2) self._executor.add_node(self) self._executor.add_node(self.__tf2_listener.node) self._executor_thread = Thread(target=self._executor.spin, args=(), daemon=True) self._executor_thread.start()
def __init__(self) -> None: """ Class constructor for visuals and graphics node Args: Returns: """ # --------------------------------------------------------------------- Thread.__init__(self) Node.__init__(self, node_name="visuals_node") # Allow callbacks to be executed in parallel without restriction. self.callback_group = ReentrantCallbackGroup() # --------------------------------------------------------------------- # Window properties self._win_name = "planner_window" self._win_size = (640, 480) # maps window shape/size self._win_time = 100 # Read back ground map self._win_background_path = "/workspace/planner/media/images/map.jpg" self._win_background = cv2.imread(self._win_background_path) self._kiwibot_img_path = "/workspace/planner/media/images/kiwibot.png" self._kiwibot_img = cv2.imread(self._kiwibot_img_path, cv2.IMREAD_UNCHANGED) # --------------------------------------------------------------------- # Subscribers self.msg_planner = planner_msg() # TODO: Implement the path planner status subscriber, # topic name: "/path_planner/msg" # message type: planner_msg # callback:cb_path_planner # add here your solution # ------------------------------------------ # TODO: Implement the Kiwibot status subscriber, # topic name: "/kiwibot/status" # message type: kiwibot_msg # callback:cb_kiwibot_status # add here your solution self.msg_kiwibot = kiwibot_msg() # ------------------------------------------ self.turn_robot( heading_angle=float(os.getenv("BOT_INITIAL_YAW", default=0.0))) self.msg_kiwibot.pos_x = int(os.getenv("BOT_INITIAL_X", default=917)) self.msg_kiwibot.pos_y = int(os.getenv("BOT_INITIAL_Y", default=1047)) # --------------------------------------------------------------------- # Publishers # Uncomment # Publisher for activating the rear cam streaming # self.msg_path_number = Int32() # self.pub_start_routine = self.create_publisher( # msg_type=Int32, # topic="/graphics/start_routine", # qos_profile=1, # callback_group=self.callback_group, # ) # --------------------------------------------------------------------- self.damon = True self.run_event = Event() self.run_event.set() self.start()
def __init__(self, node_name='introspection_client', **kwargs): Node.__init__(self, node_name, **kwargs) self._executor = SingleThreadedExecutor() self._executor.add_node(self) self._spinner = threading.Thread(target=self._executor.spin) self._spinner.start()
def __init__(self) -> None: """ Class constructor for Kiwibot Node Args: Returns: """ # --------------------------------------------------------------------- Node.__init__(self, node_name="kiwibot_node") # Allow callbacks to be executed in parallel without restriction. self.callback_group = ReentrantCallbackGroup() self._TURN_PRINT_WAYPOINT = int( os.getenv("TURN_PRINT_WAYPOINT", default=0)) self._FORWARE_PRINT_WAYPOINT = int( os.getenv("FORWARE_PRINT_WAYPOINT", default=0)) # --------------------------------------------------------------------- self.status = kiwibot_status() self.status.yaw = float(os.getenv("BOT_INITIAL_YAW", default=0.0)) self.status.pos_x = int(os.getenv("BOT_INITIAL_X", default=917)) self.status.pos_y = int(os.getenv("BOT_INITIAL_Y", default=1047)) """ kiwibot_status: int8 pos_x # x axis position in the map int8 pos_y # y axis position in the map float32 dist # distance traveled by robot float32 speed # speed m/s float32 time # time since robot is moving float32 yaw # time since robot is moving bool moving # Robot is moving """ # --------------------------------------------------------------------- # Publishers self.pub_bot_status = self.create_publisher( msg_type=kiwibot_status, topic="/kiwibot/status", qos_profile=qos_profile_sensor_data, callback_group=self.callback_group, ) self.pub_speaker = self.create_publisher( msg_type=Int8, topic="/device/speaker/command", qos_profile=qos_profile_sensor_data, callback_group=self.callback_group, ) # --------------------------------------------------------------------- # Services # service to turn the robot self.srv_robot_turn = self.create_service( Turn, "/robot/turn", self.cb_srv_robot_turn, callback_group=self.callback_group, ) # service to move the robot self.srv_robot_move = self.create_service( Move, "/robot/move", self.cb_srv_robot_move, callback_group=self.callback_group, )
def __init__(self, node_name, **kwargs): Node.__init__(self, node_name, **kwargs) self.__executor = SingleThreadedExecutor()