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()
