def create_marker(node: rclpy.node.Node,
x: float,
y: float,
z: float,
diameter: float = 0.02,
id: int = 0) -> bool:
if "client" not in create_marker.__dict__:
create_marker.client = node.create_client(CreateMarker,
'/create_marker')
if "req" not in create_marker.__dict__:
create_marker.req = CreateMarker.Request()
create_marker.req.roll = 0.0
create_marker.req.pitch = 0.0
create_marker.req.yaw = 0.0
if create_marker.client.wait_for_service(timeout_sec=5.0):
create_marker.req.id = id
create_marker.req.diameter = diameter
create_marker.req.x = x
create_marker.req.y = y
create_marker.req.z = z
# node.get_logger().debug('Calling service /create_marker')
srv_call = create_marker.client.call_async(create_marker.req)
while rclpy.ok():
if srv_call.done():
# node.get_logger().debug('Move status: %s' % srv_call.result().status_message)
break
rclpy.spin_once(node)
return srv_call.result().success
node.get_logger().error('Service /create_marker unavailable')
return False
def start(self):
from rclpy.node import get_logger
get_logger(str(self.threadID) + " " +
str(self.name)).info("Starting Thread for code \n" +
str(self.code))
self.__run_backup = self.run
self.run = self.__run
threading.Thread.start(self)
def main(args=None):
rclpy.init(args=args)
node = ReliableOdomNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
node.get_logger().info('ctrl-C detected, shutting down')
finally:
node.destroy_node()
rclpy.shutdown()
def compute_transforms(rmf_coordinates, mir_coordinates, node=None):
"""Get transforms between RMF and MIR coordinates."""
transforms = {
'rmf_to_mir': nudged.estimate(rmf_coordinates, mir_coordinates),
'mir_to_rmf': nudged.estimate(mir_coordinates, rmf_coordinates)
}
if node:
mse = nudged.estimate_error(transforms['rmf_to_mir'], rmf_coordinates,
mir_coordinates)
node.get_logger().info(f"Transformation estimate error: {mse}")
return transforms
def highlight(block, AtlDebugLevel = 0): #defined in ../atlide-blockly/js/code.js and ../nodejs/headless.js
try:
blocksClient.send(block)
except:
get_logger("atlide_core.highlight").info("Ups, cannot send message!!")
pass
from rclpy.node import get_logger
get_logger("atlide_core.highlight").info(str(block) + " " + str(AtlDebugLevel))
if AtlDebugLevel == 1:
time.sleep(1)
if AtlDebugLevel == 2:
time.sleep(2)
if AtlDebugLevel == 3:
time.sleep(5)
def post(self):
remote_ip = self.request.remote_ip
data = self.request.body
get_logger('RobotSubmitHandler').info('Receiving code: \n' + str(data) + "\n\nfrom " +str(remote_ip))
try:
code = str(data, 'utf-8')
print('Received code: \n\n' + str(code) + '\n\n')
target_bot = "TODO: Get target Bot"
action = "RUN"
call_session_manager_service(target_bot, remote_ip, code, action)
call_python_code_executor_runcode_service(target_bot, remote_ip, code, action)
except Exception as e:
err = str(e)
print('ERROR: \n\n' + str(err) + '\n\n')
raise tornado.web.HTTPError(500, err, reason=err)
def random_positions(node: rclpy.node.Node) -> Optional[numpy.ndarray]:
if "client" not in random_positions.__dict__:
random_positions.client = node.create_client(RandomPositions,
'/random_positions')
if random_positions.client.wait_for_service(timeout_sec=5.0):
# node.get_logger().debug('Calling service /create_marker')
srv_call = random_positions.client.call_async(
RandomPositions.Request())
while rclpy.ok():
if srv_call.done():
break
rclpy.spin_once(node)
# Sort the joint and position pairs such that arm_1_joint is first, and arm_3_joint is last
joint_val_pairs = list(
zip(srv_call.result().joints,
srv_call.result().positions))
joint_val_pairs.sort()
positions = [v for _, v in joint_val_pairs]
return numpy.array(positions)
node.get_logger().error('Service /random_positions unavailable')
return None
def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time):
# Profile and traits
fleet_config = config_yaml['rmf_fleet']
profile = traits.Profile(
geometry.make_final_convex_circle(
fleet_config['profile']['footprint']),
geometry.make_final_convex_circle(fleet_config['profile']['vicinity']))
vehicle_traits = traits.VehicleTraits(
linear=traits.Limits(*fleet_config['limits']['linear']),
angular=traits.Limits(*fleet_config['limits']['angular']),
profile=profile)
vehicle_traits.differential.reversible = fleet_config['reversible']
# Battery system
voltage = fleet_config['battery_system']['voltage']
capacity = fleet_config['battery_system']['capacity']
charging_current = fleet_config['battery_system']['charging_current']
battery_sys = battery.BatterySystem.make(voltage, capacity,
charging_current)
# Mechanical system
mass = fleet_config['mechanical_system']['mass']
moment = fleet_config['mechanical_system']['moment_of_inertia']
friction = fleet_config['mechanical_system']['friction_coefficient']
mech_sys = battery.MechanicalSystem.make(mass, moment, friction)
# Power systems
ambient_power_sys = battery.PowerSystem.make(
fleet_config['ambient_system']['power'])
tool_power_sys = battery.PowerSystem.make(
fleet_config['tool_system']['power'])
# Power sinks
motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys)
ambient_sink = battery.SimpleDevicePowerSink(battery_sys,
ambient_power_sys)
tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys)
nav_graph = graph.parse_graph(nav_graph_path, vehicle_traits)
# Adapter
fleet_name = fleet_config['name']
adapter = adpt.Adapter.make(f'{fleet_name}_fleet_adapter')
if use_sim_time:
adapter.node.use_sim_time()
assert adapter, ("Unable to initialize fleet adapter. Please ensure "
"RMF Schedule Node is running")
adapter.start()
time.sleep(1.0)
fleet_handle = adapter.add_fleet(fleet_name, vehicle_traits, nav_graph)
if not fleet_config['publish_fleet_state']:
fleet_handle.fleet_state_publish_period(None)
# Account for battery drain
drain_battery = fleet_config['account_for_battery_drain']
recharge_threshold = fleet_config['recharge_threshold']
recharge_soc = fleet_config['recharge_soc']
finishing_request = fleet_config['task_capabilities']['finishing_request']
node.get_logger().info(f"Finishing request: [{finishing_request}]")
# Set task planner params
ok = fleet_handle.set_task_planner_params(battery_sys, motion_sink,
ambient_sink, tool_sink,
recharge_threshold, recharge_soc,
drain_battery, finishing_request)
assert ok, ("Unable to set task planner params")
task_capabilities = []
if fleet_config['task_capabilities']['loop']:
node.get_logger().info(
f"Fleet [{fleet_name}] is configured to perform Loop tasks")
task_capabilities.append(TaskType.TYPE_LOOP)
if fleet_config['task_capabilities']['delivery']:
node.get_logger().info(
f"Fleet [{fleet_name}] is configured to perform Delivery tasks")
task_capabilities.append(TaskType.TYPE_DELIVERY)
if fleet_config['task_capabilities']['clean']:
node.get_logger().info(
f"Fleet [{fleet_name}] is configured to perform Clean tasks")
task_capabilities.append(TaskType.TYPE_CLEAN)
# Callable for validating requests that this fleet can accommodate
def _task_request_check(task_capabilities, msg: TaskProfile):
if msg.description.task_type in task_capabilities:
return True
else:
return False
fleet_handle.accept_task_requests(
partial(_task_request_check, task_capabilities))
# Transforms
rmf_coordinates = config_yaml['reference_coordinates']['rmf']
robot_coordinates = config_yaml['reference_coordinates']['robot']
transforms = {
'rmf_to_robot': nudged.estimate(rmf_coordinates, robot_coordinates),
'robot_to_rmf': nudged.estimate(robot_coordinates, rmf_coordinates)
}
transforms['orientation_offset'] = \
transforms['rmf_to_robot'].get_rotation()
mse = nudged.estimate_error(transforms['rmf_to_robot'], rmf_coordinates,
robot_coordinates)
print(f"Coordinate transformation error: {mse}")
print("RMF to Robot transform:")
print(f" rotation:{transforms['rmf_to_robot'].get_rotation()}")
print(f" scale:{transforms['rmf_to_robot'].get_scale()}")
print(f" trans:{transforms['rmf_to_robot'].get_translation()}")
print("Robot to RMF transform:")
print(f" rotation:{transforms['robot_to_rmf'].get_rotation()}")
print(f" scale:{transforms['robot_to_rmf'].get_scale()}")
print(f" trans:{transforms['robot_to_rmf'].get_translation()}")
def _updater_inserter(cmd_handle, update_handle):
"""Insert a RobotUpdateHandle."""
cmd_handle.update_handle = update_handle
# Initialize robots for this fleet
robots = {}
for robot_name, robot_config in config_yaml['robots'].items():
node.get_logger().info(f"Initializing robot:{robot_name}")
rmf_config = robot_config['rmf_config']
robot_config = robot_config['robot_config']
initial_waypoint = rmf_config['start']['waypoint']
initial_orientation = rmf_config['start']['orientation']
robot = RobotCommandHandle(
name=robot_name,
config=robot_config,
node=node,
graph=nav_graph,
vehicle_traits=vehicle_traits,
transforms=transforms,
map_name=rmf_config['start']['map_name'],
initial_waypoint=initial_waypoint,
initial_orientation=initial_orientation,
charger_waypoint=rmf_config['charger']['waypoint'],
update_frequency=rmf_config.get('robot_state_update_frequency', 1),
adapter=adapter)
if robot.initialized:
robots[robot_name] = robot
# Add robot to fleet
fleet_handle.add_robot(robot, robot_name, profile, robot.starts,
partial(_updater_inserter, robot))
node.get_logger().info(
f"Successfully added new robot:{robot_name}")
else:
node.get_logger().error(f"Failed to initialize robot:{robot_name}")
return adapter, fleet_handle, robots
def open(self):
get_logger('WSBlocksSPub').info('WSBlocksSPub Handler client connected')
def on_message(self, message):
get_logger('WSBlocksSubHandler').info('WSBlocksSub client message: ' + message)
# echo message back to client
self.write_message(message)
def on_close(self):
get_logger('WSBlocksSubHandler').info('WSBlocksSub client disconnected')
def open(self):
get_logger('WSBlocksSubHandler').info('WSBlocksSub client connected')
self.application._blocks = self
def post(self):
remote_ip = self.request.remote_ip
get_logger('RobotTerminateHandler').info('Request from ' + str(remote_ip) + " to terminate program")
target_bot = "TODO: Get target Bot"
action = "STOP"
call_session_manager_service(target_bot, remote_ip, code, action)
def run_code(threadID, name, code):
from rclpy.node import get_logger
exec(code) # run the python code
get_logger(str(threadID) + " " + str(name)).info("Job done! Exiting " +
str(name))
def kill(self):
from rclpy.node import get_logger
get_logger(str(self.threadID) + " " +
str(self.name)).info("Killing " + str(self.name))
self.killed = True