def setUpClass(cls):
cls.context = rclpy.context.Context()
rclpy.init(context=cls.context)
cls.node0 = rclpy.create_node(TEST_NODE0,
namespace=TEST_NAMESPACE0,
context=cls.context)
cls.node1 = rclpy.create_node(TEST_NODE1,
namespace=TEST_NAMESPACE1,
context=cls.context)
cls.node2 = rclpy.create_node(TEST_NODE2,
namespace=TEST_NAMESPACE2,
context=cls.context)
cls.action_client10 = ActionClient(cls.node1, Fibonacci, TEST_ACTION0)
cls.action_server10 = ActionServer(cls.node1, Fibonacci, TEST_ACTION0,
lambda: None)
cls.action_client20 = ActionClient(cls.node2, Fibonacci, TEST_ACTION0)
cls.action_client21 = ActionClient(cls.node2, Fibonacci, TEST_ACTION1)
cls.action_server20 = ActionServer(cls.node2, Fibonacci, TEST_ACTION0,
lambda: None)
cls.action_server21 = ActionServer(cls.node2, Fibonacci, TEST_ACTION1,
lambda: None)
assert cls.wait_for_node(node=cls.node1,
remote_node=cls.node0,
timeout=2)
assert cls.wait_for_node(node=cls.node1,
remote_node=cls.node2,
timeout=2)
assert cls.wait_for_node(node=cls.node2,
remote_node=cls.node0,
timeout=2)
assert cls.wait_for_node(node=cls.node2,
remote_node=cls.node1,
timeout=2)
def __init__(self):
super().__init__('roughwalk', namespace='hapthexa')
self._leg_names = [
'front_left', 'middle_left', 'rear_left', 'rear_right',
'middle_right', 'front_right'
]
self._gait_action_clients = []
for leg_name in self._leg_names:
self._gait_action_clients.append(
ActionClient(self, Gait, 'leg/' + leg_name + '/gait'))
self._leg_group1 = []
self._leg_group2 = []
for i in range(len(self._leg_names)):
if i % 2:
self._leg_group1.append(self._leg_names[i])
else:
self._leg_group2.append(self._leg_names[i])
self.get_logger().debug('{0}'.format(len(self._leg_names)))
self._send_goal_futures = [Future] * len(self._leg_names)
self._get_result_futures = [Future] * len(self._leg_names)
self._executing_action = [False] * len(self._leg_names)
self._move_leg_action_client = ActionClient(self, MoveLeg,
'leg/front_left/move_leg')
self._z_offset = 0
self._walk_thread = threading.Thread(target=self.walk_thread)
self._walk_thread.start()
def __init__(self) -> None:
super().__init__('sorter')
# Initialize clients and wait for servers
self._nav_client: ActionClient = ActionClient(
self, NavigateToPose, 'navigate_to_pose'
)
self._picker_client: ActionClient = ActionClient(self, PickObject, 'PickObject')
self._placer_client: ActionClient = ActionClient(
self, PlaceObject, 'PlaceObject'
)
self._selector_client: ActionClient = ActionClient(
self, SelectObject, 'SelectObject'
)
self._picker_client.wait_for_server()
self._placer_client.wait_for_server()
self._selector_client.wait_for_server()
self._nav_client.wait_for_server()
self.get_logger().info('Sorter ready.')
# Used to check goal status against e.g. self.goal_result.index('succeeded')
self.goal_result: Tuple[str, ...] = (
"unknown",
"accepted",
"executing",
"canceling",
"succeeded",
"canceled",
"aborted",
) # There should be some way to translate goal status codes to english in rclpy
self.declare_parameter('behavior_tree')
self.bt: str = self.get_parameter(
'behavior_tree'
).get_parameter_value().string_value
def __init__(self):
super().__init__('R_KR16')
self.publisher_ = self.create_publisher(AdjacencyList, 'graph_node_network', 10)
timer_period = 1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
#Action server declaration
self._action_server = ActionServer(self,Transport,'transport_request',self.transport_callback,goal_callback=self.goal_parse_msg)
self.action_storage_client_2 = ActionClient(self, Storage, 'storage_request_2')
self.action_storage_client_0 = ActionClient(self, Storage, 'storage_request_0')
self.graph = AdjacencyList()
self.graph.node = 1
self.graph.adjacent = [0,2]
self.storage_0_ready = 0
self.storage_2_ready = 0
print("init.....")
#--------GPIO pin setup start-------------
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
self.pin_start_01 = 36
self.pin_start_12 = 37
self.pin_progress = 31
self.pin_finished = 33
GPIO.setup(self.pin_start_01, GPIO.OUT)
GPIO.setup(self.pin_start_12, GPIO.OUT)
GPIO.setup(self.pin_progress, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(self.pin_finished, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.output(self.pin_start_01, GPIO.LOW)
GPIO.output(self.pin_start_12, GPIO.LOW)
#--------GPIO pin setup end-------------
print("ready")
def __init__(self):
super().__init__('joy_ctrl_record_replay_traj')
# Local variables
self.record_action_goal_handle_ = None
self.replay_action_goal_handle_ = None
self.vehicle_kinematic_state_msg_ = None
self.recorded_states_ = 0
self.replay_remaining_length_ = 0
self.vehicle_kinematic_state_msg_ = None
self.timer_period = 0.5 # seconds
self.internal_state_ = InternalState.Idle
# Actionclient
self.replay_action_client_ = ActionClient(
self, ReplayTrajectory, "/planning/replaytrajectory")
self.record_action_client_ = ActionClient(
self, RecordTrajectory, "/planning/recordtrajectory")
# Publisher / Subscriber
self.subscriber_vehicle_kinematic_state_ = self.create_subscription(
VehicleKinematicState, "vehicle_kinematic_state",
self.vehicle_kinematic_state_cb, 0)
self.subscriber_joystick_recordreplay_cmd_ = self.create_subscription(
UInt8, "recordreplay_cmd", self.joystickStateCb, 0)
self.timer = self.create_timer(self.timer_period, self.set_action)
def __init__(self, robot_name):
super().__init__('control_node_' + robot_name)
self.cmd_vel_pub_ = self.create_publisher(Twist,
robot_name + '/cmd_vel', 10)
self.compute_path_client_ = ActionClient(
self, ComputePathToPose, robot_name + '/compute_path_to_pose')
self.navigate_to_pose_client_ = ActionClient(
self, NavigateToPose, robot_name + '/navigate_to_pose')
self.swarm_simulation_navigate_to_pose_client_ = ActionClient(
self, NavigationAction, robot_name + '/navigation_action')
self.get_path_result = None
self.get_path_done_ = False
self.e_util = ExploreUtil()
self.navigate_to_pose_state_ = self.e_util.NAVIGATION_NO_GOAL
def __init__(self):
super().__init__('R_KR10')
self.publisher_ = self.create_publisher(AdjacencyList, 'graph_node_network', 10)
timer_period = 1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
#Action server declaration
self._action_server = ActionServer(self,Transport,'transport_request',self.transport_callback,goal_callback=self.goal_parse_msg)
self.action_storage_client_2 = ActionClient(self, Storage, 'storage_request_2')
self.action_storage_client_5 = ActionClient(self, Storage, 'storage_request_5')
self.action_storage_client_6 = ActionClient(self, Storage, 'storage_request_6')
self.servcie_machine_client_4 = self.create_client(MachineRequest5, 'machine_request_5')
self.graph = AdjacencyList()
self.graph.node = 3
self.graph.adjacent = [2,4,5,6]
self.storage_2_ready = 0
print("init.....")
#--------GPIO pin setup start-------------
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
self.pin_start_23 = 33
self.pin_start_32 = 35
self.pin_start_34 = 36
self.pin_start_43 = 37
self.pin_start_35 = 38
self.pin_start_36 = 40
self.pin_progress = 16
self.pin_finished = 18
GPIO.setup(self.pin_start_23, GPIO.OUT) #Input 2 Kuka
GPIO.setup(self.pin_start_32, GPIO.OUT) #Input 3 Kuka
GPIO.setup(self.pin_start_34, GPIO.OUT) #Input 4 Kuka
GPIO.setup(self.pin_start_43, GPIO.OUT) #Input 5 Kuka
GPIO.setup(self.pin_start_35, GPIO.OUT) #Input 6 Kuka
GPIO.setup(self.pin_start_36, GPIO.OUT) #Input 7 Kuka
GPIO.setup(self.pin_progress, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(self.pin_finished, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.output(self.pin_start_23, GPIO.LOW)
GPIO.output(self.pin_start_32, GPIO.LOW)
GPIO.output(self.pin_start_34, GPIO.LOW)
GPIO.output(self.pin_start_43, GPIO.LOW)
GPIO.output(self.pin_start_35, GPIO.LOW)
GPIO.output(self.pin_start_36, GPIO.LOW)
#--------GPIO pin setup end-------------
print("ready")
def __init__(self):
"""Init Node."""
self.node_name = 'gamepad_parser_node'
super().__init__(self.node_name)
# Init Params
self.init_params()
# Create Publishers
self.rover_motion_cmd_pub = self.create_publisher(
Twist, 'rover_motion_cmd', 10)
self.ptu_cmd_pub = self.create_publisher(Twist, 'ptu_cmd', 10)
# Create Subscriptions
self.create_subscription(Joy, 'gamepad', self.gamepad_callback, 10)
# Request Locomotion Mode Service
self.change_locomotion_mode_cli = self.create_client(
ChangeLocomotionMode, 'change_locomotion_mode')
# Nav to Pose Action Client
self.nav_to_pose_cli = ActionClient(self, NavigateToPose,
'navigate_to_pose')
self.request = ChangeLocomotionMode.Request()
self.client_futures = []
self.get_logger().info('\t{} STARTED.'.format(self.node_name.upper()))
def __init__(self, node_name):
super().__init__(node_name)
self.client = ActionClient(self, NavigateToPose, "NavGoal")
self.goal_nbr = 0
self.goal = None
self.robot_pose_img = Point()
self.target = Point()
self.call = None
self.map_data = None
self.width = 0
self.height = 0
self.map_origin = Point()
self.resolution = 0
self.robot_yaw = 0
self.robot_pose = Point()
self.tf_buffer = Buffer()
self.tf_timer = None
self.listener = TransformListener(self.tf_buffer, self)
try:
self.map_srv = self.create_client(GetMap, "/map_server/map")
self.get_logger().info("Service client created")
while not self.map_srv.wait_for_service(timeout_sec=1.0):
self.get_logger().info(
'service not available, waiting again...')
except Exception as e:
self.get_logger().info(e)
self.get_logger().info("robot initialization finished")
def __init__(self):
super().__init__('pipeline_sequence_manager')
self.declare_parameters(namespace='',
parameters=[
('pipeline_sequence', []),
])
self.pipelines = self.get_parameter(
'pipeline_sequence').get_parameter_value().string_array_value
self.get_logger().info('Pipeline Sequence ' + str(self.pipelines))
self.configure_client = self.create_client(
ConfigurePipeline, '/triton/configure_pipeline')
self.run_client = ActionClient(self, RunPipeline,
'/triton/run_pipeline')
while not self.run_client.wait_for_server(timeout_sec=1.0):
self.get_logger().warn(
'Run action not available, make sure you have launched the pipeline...'
)
while not self.configure_client.wait_for_service(timeout_sec=1.0):
self.get_logger().warn(
'Configure service not available, make sure you have launched the pipeline...'
)
self.get_logger().info(
'Pipeline Sequence Manager successfully started!')
def test_send_goal_async_with_feedback_for_another_goal(self):
ac = ActionClient(self.node, Fibonacci, 'fibonacci')
try:
self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
# Send a goal and then publish feedback
first_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
future = ac.send_goal_async(
Fibonacci.Goal(),
feedback_callback=self.feedback_callback,
goal_uuid=first_goal_uuid)
rclpy.spin_until_future_complete(self.node, future, self.executor)
# Send another goal, but without a feedback callback
second_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
future = ac.send_goal_async(Fibonacci.Goal(),
goal_uuid=second_goal_uuid)
rclpy.spin_until_future_complete(self.node, future, self.executor)
# Publish feedback for the second goal
self.mock_action_server.publish_feedback(second_goal_uuid)
self.timed_spin(1.0)
self.assertEqual(self.feedback, None)
# Publish feedback for the first goal (with callback)
self.mock_action_server.publish_feedback(first_goal_uuid)
self.timed_spin(1.0)
self.assertNotEqual(self.feedback, None)
finally:
ac.destroy()
async def service_callback(self, request, response, goal_handle):
position = request["position"]
orientation = request["orientation"]
self.action_client = ActionClient(self,
NavigateToPose,
'/NavigateToPose',
callback_group=self.cb_group)
if not self.action_client.wait_for_server(timeout_sec=5.0):
response.message = "Abort"
return response
goal_msg = NavigateToPose.Goal()
goal_msg.pose.header.frame_id = "map"
goal_msg.pose.pose.position.x = position[0]
goal_msg.pose.pose.position.y = position[1]
goal_msg.pose.pose.position.z = position[2]
goal_msg.pose.pose.orientation.x = orientation[0]
goal_msg.pose.pose.orientation.y = orientation[1]
goal_msg.pose.pose.orientation.z = orientation[2]
goal_msg.pose.pose.orientation.w = orientation[3]
self.move_goalhandle = await self.action_client.send_goal_async(
goal_msg)
if not self.move_goalhandle.accepted:
self.get_logger().info("goal rejected")
response.message = "Abort"
return response
future_result = await self.move_goalhandle.get_result_async()
response.message = "Success"
return response
def __init__(self):
super().__init__('indention_tester')
self._ttac3_action_client = ActionClient(
self, TTAC3, 'ttac3'
)
# Main Loop (by Thread)
update_period = 0.5
self.main_thread = create_thread(update_period, self.update)
# Publish Target Air Pressure
self.pub_target_air_pressure = self.create_publisher(Float32,'target_air_pressure', 10)
pub_timer = 0.5 # seconds
self.target_air_pressure = 150.0 # target_air_pressure
self.timer = self.create_timer(pub_timer, self.pub_target_air_pressure_callback)
# Subscribe Current Air Pressure
self.sub_current_air_pressure = self.create_subscription(
Float32,
'current_air_pressure',
self.current_air_pressure_listener_callback,
10
)
# Subscribe Bend Sensor
self.sub_bend_sensor = self.create_subscription(
Float32,
'bend_sensor',
self.bend_sensor_listener_callback,
10
)
def __init__(self):
super().__init__('route_manager', allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
self.route = []
self.current_goal = NavigateToPose.Goal()
self.client = ActionClient(self, NavigateToPose, 'NavigateToPose')
self.client.wait_for_server()
time.sleep(10)
route_file_info = self.get_parameter('route_file').value
# route file info is in the form "<package-name>.<path from install's share directory>"
route_pkg_share = get_package_share_directory(route_file_info.split('.')[0])
route_file_path = os.path.join(route_pkg_share, '.'.join(route_file_info.split('.')[1:]))
with open(route_file_path, 'r') as f:
route_file_contents = f.read()
route_yaml = yaml.safe_load(route_file_contents)
self.route_mode = route_yaml['mode']
if self.route_mode not in RouteManager.route_modes:
self.get_logger().error(
"Route mode '%s' unknown, exiting route manager" % (self.route_mode,))
return
poses = route_yaml['poses']
if not poses:
self.get_logger().info("Route manager initialized no goals, unable to route")
self.goals = RouteManager.route_modes[self.route_mode](poses)
self.get_logger().info(
"Route manager initialized with %s goals in %s mode" % (len(poses), self.route_mode,))
def __init__(self, node: Node):
self.node = node
self.config = self.node.get_parameter("behavior/body").get_parameter_value().double_value
self.base_footprint_frame = self.node.get_parameter("~base_footprint_frame").get_parameter_value().double_value
self.map_frame = self.node.get_parameter("~map_frame").get_parameter_value().double_value
self.blackboard = BlackboardCapsule()
self.gamestate = GameStatusCapsule()
self.animation = AnimationCapsule()
self.kick = KickCapsule(self)
self.world_model = WorldModelCapsule(self)
self.pathfinding = PathfindingCapsule(self)
self.world_model = WorldModelCapsule(self)
self.team_data = TeamDataCapsule()
# animations
self.animation_action_client = ActionClient(self, PlayAnimationAction, 'animation')
self.goalie_arms_animation = self.node.get_parameter("Animations/Goalie/goalieArms").get_parameter_value().double_value
self.goalie_falling_right_animation = self.node.get_parameter("Animations/Goalie/fallRight").get_parameter_value().double_value
self.goalie_falling_left_animation = self.node.get_parameter("Animations/Goalie/fallLeft").get_parameter_value().double_value
self.goalie_falling_center_animation = self.node.get_parameter("Animations/Goalie/fallCenter").get_parameter_value().double_value
self.cheering_animation = self.node.get_parameter("Animations/Misc/cheering").get_parameter_value().double_value
self.init_animation = self.node.get_parameter("Animations/Misc/init").get_parameter_value().double_value
self.dynup_action_client = None
self.dynup_cancel_pub = None # type: rospy.Publisher
self.hcm_deactivate_pub = None # type: rospy.Publisher
def init_robot(self):
# Initialize clients and test service appearance
self.switch_controller_client = self.node.create_client(
SwitchController, "/controller_manager/switch_controller")
if self.switch_controller_client.wait_for_service(10) is False:
raise Exception(
"Could not reach switch controller service, make sure that the controller_manager is actually running"
)
self.set_io_client = self.node.create_client(
SetIO, "/io_and_status_controller/set_io")
if self.set_io_client.wait_for_service(10) is False:
raise Exception(
"Could not reach set IO service, make sure that the driver is actually running"
)
self.resend_robot_program_client = self.node.create_client(
Trigger, "/io_and_status_controller/resend_robot_program")
if self.resend_robot_program_client.wait_for_service(10) is False:
raise Exception(
"Could not reach stop service, make sure that the driver is actually running"
)
# test action appearance
self.jtc_action_client = ActionClient(
self.node,
FollowJointTrajectory,
"/scaled_joint_trajectory_controller/follow_joint_trajectory",
)
if self.jtc_action_client.wait_for_server(10) is False:
raise Exception(
"Could not reach /scaled_joint_trajectory_controller/follow_joint_trajectory action server,"
"make sure that controller is active (load + start)")
def __init__(self, namespace: str = None):
super().__init__('each_leg_gait', namespace=namespace)
self._gait_action_server = ActionServer(
self,
Gait,
'gait',
execute_callback=self.execute_callback,
callback_group=ReentrantCallbackGroup(),
goal_callback=self.goal_callback,
cancel_callback=self.cancel_callback)
self._calibrate_signal_pub = self.create_publisher(
Empty, 'calibrate_signal', 10)
self._move_leg_action_client = ActionClient(self, MoveLeg, 'move_leg')
self._forcesensor_sub = self.create_subscription(
ForceSensor, 'force_sensor', self.forcesensor_callback, 10)
# self._timer = self.create_timer(0.001, lambda: self.get_logger().info('timer'))
self._leg_stop_condition = (lambda: False)
self._leg_stoped = False
self._move_leg_goal_handle = None
self._forcesensor_msg = None
self._x_result = 0.0
self._y_result = 0.0
self._z_result = 0.0
self._z_lock = 0.0
self._x_offset = 0
self._z_offset = 0.0
self._timer_start = time.time()
async def create_tasknode(self, task_tree):
global executor
# generate task_node
child_tasknode = TaskNode(task_tree)
# self.child_tasknode.append(self.childtask_node)
executor.add_node(child_tasknode) # added last added task
## execute
client = ActionClient(self,
TsDoTask,
child_tasknode.name,
callback_group=ReentrantCallbackGroup())
if not client.wait_for_server(timeout_sec=5.0):
self.get_logger().error('No action server available')
goal_handle.abort()
result = TsDoTask.Result()
result.message = "Abort"
return result
goal = TsDoTask.Goal()
goal_handle_client = await client.send_goal_async(goal)
if not goal_handle_client.accepted:
self.get_logger().info("goal reject")
goal_handle.abort()
result = TsDoTask.Result()
result.message = "Goal Reject"
return result
self.get_logger().info("goal accept")
return goal_handle_client, child_tasknode
def __init__(self, name, actionName):
super().__init__(name)
self.__client = ActionClient(self, FollowJointTrajectory, actionName)
self.__remaining_iteration = 0
self.__current_trajectory = None
self.__get_result_future = None
self.__send_goal_future = None
def __init__(self):
super().__init__(node_name='nav2_waypoint_tester', namespace='')
self.waypoints = None
self.action_client = ActionClient(self, FollowWaypoints,
'FollowWaypoints')
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, 'initialpose', 10)
def __init__(self):
super().__init__(node_name='nav2_waypoint_tester', namespace='')
self.waypoints = None
self.readyToMove = True
self.currentPose = None
self.lastWaypoint = None
self.action_client = ActionClient(self, FollowWaypoints, 'FollowWaypoints')
self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
'initialpose', 10)
self.costmapClient = self.create_client(GetCostmap, '/global_costmap/get_costmap')
while not self.costmapClient.wait_for_service(timeout_sec=1.0):
self.info_msg('service not available, waiting again...')
self.initial_pose_received = False
self.goal_handle = None
pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(Odometry,
'/odom', self.poseCallback, pose_qos)
# self.costmapSub = self.create_subscription(Costmap(), '/global_costmap/costmap_raw', self.costmapCallback, pose_qos)
self.costmapSub = self.create_subscription(OccupancyGrid(), '/map', self.occupancyGridCallback, pose_qos)
self.costmap = None
self.get_logger().info('Running Waypoint Test')
async def subtask(self, goal_handle, task_tree):
# print("subtask")
command = task_tree[1]
subtask_client = ActionClient(self,
TsDoSubtask,
"subtask_node_" + str(command[0]),
callback_group=ReentrantCallbackGroup())
if not subtask_client.wait_for_server(timeout_sec=5.0):
self.get_logger().error('No action server available')
goal_handle.abort()
result = TsReq.Result()
result.message = "Abort"
return result
goal_msg = TsDoSubtask.Goal()
if len(command) >= 2:
goal_msg.arg_json = command[1]
else:
goal_msg.arg_json = "{}"
print(f'subtask: subtask_node_{command[0]} args:{goal_msg.arg_json}')
subtask_goalhandle = await subtask_client.send_goal_async(goal_msg)
self.subtask_goalhandles[
subtask_goalhandle.goal_id.uuid.tostring()] = subtask_goalhandle
if not subtask_goalhandle.accepted:
self.get_logger().info("goal rejected")
del self.subtask_goalhandles[
subtask_goalhandle.goal_id.uuid.tostring()]
return "Abort"
future_result = await subtask_goalhandle.get_result_async()
del self.subtask_goalhandles[
subtask_goalhandle.goal_id.uuid.tostring()]
self.get_logger().info(f"returned {future_result.result.message}")
return future_result.result.message
def __init__(self):
super().__init__('plan_trajectory_client_mock')
self._action_client = ActionClient(self, PlanTrajectory,
'plan_parking_trajectory')
self._send_goal_future = None
self._get_result_future = None
self._result = None
def __init__(self, ns='l2'):
self.name = 'tasks'
super().__init__(self.name, namespace=ns)
self.get_logger().info('Init')
self.get_logger().info('Setting up service')
with open('/config/todoist.json', 'r') as f:
self.config = json.loads(f.read())
self.get_logger().info('Config loaded')
self.seq_queue = [] # Extracted sequences from tasks
self.tasks_with_commands = set(
) # Note: not all tasks have valid commands
self.active_project_sub = self.create_service(l2.L2ActiveProjects,
'set_active_project',
handle_active_project)
self.active_project_pub = self.create_publisher(
l2.L2ActiveProjects, 'active_projects', 10)
self.pub = self.create_publisher(l2.Projects, 'projects', 10)
self.seqcli = ActionClient(self, L2SequenceAction, 'sequence')
self.timer = self.create_timer(self.PUBLISH_PD, self.timer_callback)
self.ticks = 0
# Active projects map (held in memory)
# Each user can have exactly one active project
self.active_projects = {}
# Init the todoist client and prime the system
self.setup_todoist()
self.timer_callback()
def __init__(self, action_type, node, server_name):
self._server_name = server_name
if action_type == "Fibonacci":
self._action_client = ActionClient(node, Fibonacci, server_name)
self.__result = None
self.__state = "Free"
def __init__(self,
initial_pose: Pose,
goal_pose: Pose,
namespace: str = ''):
super().__init__(node_name='nav2_tester', namespace=namespace)
self.initial_pose_pub = self.create_publisher(
PoseWithCovarianceStamped, 'initialpose', 10)
self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10)
pose_qos = QoSProfile(
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
depth=1)
self.model_pose_sub = self.create_subscription(
PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback,
pose_qos)
self.initial_pose_received = False
self.initial_pose = initial_pose
self.goal_pose = goal_pose
self.action_client = ActionClient(self, NavigateToPose,
'navigate_to_pose')
def __init__(self):
super().__init__('rough_walk')
self._leg_names = [
'front_left', 'middle_left', 'rear_left', 'rear_right',
'middle_right', 'front_right'
]
self._leg_args = [
pi / 6.0, pi / 2.0, pi * 5.0 / 6.0, -pi * 5.0 / 6.0, -pi / 2.0,
-pi / 6.0
]
self._phase = 0
self._move_succeed_leg_count = 0
self._w = 8.0
self._h = 12.0
self._z_lock = 0.0
self._z_offset = 0.0
self._z_height = 0.0
self._exit = False
signal.signal(signal.SIGINT, self.sigint_callback)
self._action_clients = []
for leg_name in self._leg_names:
self._action_clients.append(
ActionClient(self, MoveLeg,
'hapthexa/leg/' + leg_name + '/move_leg'))
self._send_goal_future = [0] * 6
self._get_result_future = [0] * 6
self._z_result = [0.0] * 6
self._z_lock = [0.0] * 6
for i in range(6):
self.generate_trajectory(i, 6 if i % 2 else 3)
def __init__(self):
"""Constructor for client node
"""
super().__init__('Hydrophone_localiser_aclient')
self._action_client = ActionClient(self, Hydrophonelocalise,
'hydrophonelocaliser')
self.designated_goal = String()
self.designated_goal.data = "both"
def __init__(self):
super().__init__(node_name="navigator")
self.goal_handle = None
self.result_future = None
self.feedback = None
self.status = None
self.nav_to_pose_client = ActionClient(self, NavigateToPose,
"navigate_to_pose")
def __init__(self):
super().__init__('maze_action_client')
self.declare_parameter('robot_namespace', 'diffbot')
self.robot_namespace = self.get_parameter('robot_namespace').value + '/'
self.action_client = ActionClient(self, Maze, self.robot_namespace + 'maze_action')
self.get_logger().info('=== Maze Action Client Started ====')