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 ====')