def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False self._action_topic = userdata.move_group_prefix + userdata.action_topic self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._move_group = userdata.move_group self._joint_names = None self._joint_names = userdata.joint_names action_goal = MoveGroupGoal() action_goal.request.group_name = self._move_group goal_constraints = Constraints() for i in range(len(self._joint_names)): goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_values[i])) action_goal.request.goal_constraints.append(goal_constraints) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True
def __init__(self, planner, tolerance=0.2, controller, recovery_behaviors=[]): super(MoveBaseActionState, self).__init__(output_keys=[ 'outcome', 'final_pose', 'dist_to_goal', 'angle_to_goal' ], input_keys=['target_pose'], outcomes=['succeeded', 'failed', 'aborted']) self._planner = planner self._tolerance = tolerance self._controller = controller self._recovery_behaviors = recovery_behaviors # Create the action client when building the behavior. self._topic = '/move_base_flex/move_base' self._client = ProxyActionClient( {self._topic: MoveBaseAction}) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal. self._error = False
def __init__(self, controller='motion/controller/joint_state_head', tolerance=0.17, timeout=10.0, joint_topic="joint_states", outcomes=['done', 'failed', 'timeout']): super(SetJointStateBase, self).__init__(outcomes=outcomes) # Store topic parameter for later use. self._controller = controller self._joint_topic = joint_topic self._tolerance = tolerance self._timeout = Duration.from_sec(timeout) # create proxies self._action_client = ProxyActionClient( {self._controller: SetOperationalAction}) self._pose_publisher = ProxyPublisher( {self._controller + '/in_joints_ref': JointState}) self._pose_subscriber = ProxySubscriberCached( {self._joint_topic: JointState}) # timestamp self._timestamp = None # error in enter hook self._error = False
def __init__(self): """Constructor""" super(MoveBaseState, self).__init__(outcomes=['arrived', 'failed'], input_keys=['Direction', 'curr_pose']) self._action_topic = "/move_base" self._client = ProxyActionClient({self._action_topic: MoveBaseAction}) self._arrived = False self._failed = False # Direction and Correction Parameters self.origin = [-12.0, -8.0] # self.origin = [4.15, 17.4] self.quadrant = 1 self.turn_metric = [3.25, 2.5] self.straight_metric = 2.0 self.back_metric = 1.0 # Locations of the Goal and the Charging Stations self.charging_station_1 = [-8.32, 4.97] self.charging_station_2 = [6.79, 1.03] self.parking_sign = [11.0, -3.0] self.parking_point = [13.125, -9.36]
def __init__(self): smach.State.__init__(self, outcomes=['Success', 'Failure'], input_keys=['args']) self.topic = "move_distance" self.client = ProxyActionClient({ self.topic:riptide_controllers.msg.MoveDistanceAction})
def __init__(self, timeout=5.0, max_delay=-1.0, wait_duration=5, action_topic=None): ''' Constructor ''' super(ExecuteKnownTrajectoryState, self).__init__( input_keys=['action_topic', 'trajectory'], outcomes=['done', 'failed', 'param_error'], output_keys=['status_text', 'goal_names', 'goal_values'] ) self.action_client = None self.goal_names = None self.goal_values = None self.timeout_duration = rospy.Duration(timeout) self.timeout_target = None self.wait_duration = wait_duration self.given_action_topic = action_topic self.max_delay = None if max_delay > 0.0: self.max_delay = rospy.Duration(max_delay) try: if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0): # If topic is defined, set the client up on startup self.action_client = ProxyActionClient({self.given_action_topic: ExecuteKnownTrajectoryAction}, self.wait_duration) else: self.given_action_topic = None # override "" except Exception as e: Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e))) self.current_action_topic = self.given_action_topic self.status_text = None self.return_code = None
def __init__(self): smach.State.__init__(self, outcomes=['Success', 'Failure'], input_keys=['args']) self.topic = "go_to_yaw" self.client = ProxyActionClient( {self.topic, riptide_controllers.msg.GoToYawAction})
def __init__(self, direction): ''' Constructor ''' super(FootstepPlanRelativeState, self).__init__(outcomes=['planned', 'failed'], input_keys=['distance'], output_keys=['plan_header']) if not rospy.has_param("behavior/step_distance_forward"): Logger.logerr( "Need to specify parameter behavior/step_distance_forward at the parameter server" ) return if not rospy.has_param("behavior/step_distance_sideward"): Logger.logerr( "Need to specify parameter behavior/step_distance_sideward at the parameter server" ) return self._step_distance_forward = rospy.get_param( "behavior/step_distance_forward") self._step_distance_sideward = rospy.get_param( "behavior/step_distance_sideward") self._action_topic = '/vigir/footstep_manager/step_plan_request' self._client = ProxyActionClient( {self._action_topic: StepPlanRequestAction}) self._done = False self._failed = False self._direction = direction
def __init__(self, desired_tilt): '''Constructor''' super(TiltHeadState, self).__init__(outcomes=['done', 'failed']) if not rospy.has_param("behavior/joint_controllers_name"): Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server") return controller_namespace = rospy.get_param("behavior/joint_controllers_name") # self._configs['flor']['same'] = { # 20: {'joint_name': 'neck_ry', 'joint_values': [+0.00], 'controller': 'neck_traj_controller'}, # max -40 degrees # 21: {'joint_name': 'neck_ry', 'joint_values': [-40.0], 'controller': 'neck_traj_controller'}, # 22: {'joint_name': 'neck_ry', 'joint_values': [-20.0], 'controller': 'neck_traj_controller'}, # 23: {'joint_name': 'neck_ry', 'joint_values': [+20.0], 'controller': 'neck_traj_controller'}, # 24: {'joint_name': 'neck_ry', 'joint_values': [+40.0], 'controller': 'neck_traj_controller'}, # 25: {'joint_name': 'neck_ry', 'joint_values': [+60.0], 'controller': 'neck_traj_controller'} # max +60 degrees # } self._action_topic = "/" + controller_namespace + \ "/neck_traj_controller" + "/follow_joint_trajectory" self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction}) # Convert desired position to radians self._desired_tilt = math.radians(desired_tilt) self._failed = False
def on_enter(self, userdata): self.return_code = None # Retrieve the relevant data if self.given_action_topic == None: # Not action topic define, check userdata try : self.current_action_topic = userdata.action_topic except Exception as e: Logger.logwarn('Failed to get relevant user data -\n%s' % (str(e))) self.return_code = 'failed' return try: if (self.client is None): self.client = ProxyActionClient({self.current_action_topic: ClearOctomapAction}, self.wait_duration) except Exception as e: Logger.logwarn('Failed to set up the action client for %s\n%s' % (self.current_action_topic, str(e))) self.return_code = 'failed' return try: # Action Initialization action_goal = ClearOctomapGoal() self.client.send_goal(self.current_action_topic, action_goal) if self.timeout_duration > rospy.Duration(0.0): self.timeout_target = rospy.Time.now() + self.timeout_duration else: self.timeout_target = None except Exception as e: Logger.logwarn('Failed to send action goal for group - %s\n%s' % (self.name, str(e))) self.return_code = 'failed'
def __init__(self, config_name='Home', move_group='arm', action_topic='/move_group', robot_name='m1n6s200', position_topic='/m1n6s200_driver/joint_states', delta=1E-4): ''' Constructor ''' super(FeedbackSrdfStateToMoveit, self).__init__(outcomes=['reached', 'failed']) self._position_topic = position_topic self._delta = delta self._config_name = config_name self._move_group = move_group self._robot_name = robot_name self._action_topic = action_topic self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._planning_failed = False self._control_failed = False self._success = False self._srdf_param = None if rospy.has_param("/robot_description_semantic"): self._srdf_param = rospy.get_param("/robot_description_semantic") else: Logger.logerror('Unable to get parameter: /robot_description_semantic') self._param_error = False self._srdf = None
def __init__(self, topic): """Constructor""" super(BigGateManeuverState, self).__init__(outcomes=['Success', 'Failure']) self._topic = topic self.client = ProxyActionClient( {self._topic: riptide_controllers.msg.GateManeuverAction})
def __init__(self, timeout=5.0, wait_duration=5, action_topic=None): ''' Constructor ''' super(ApplyPlanningSceneState, self).__init__( input_keys=['action_topic'], outcomes=['done', 'failed']) self.client = None self.timeout_duration = rospy.Duration(timeout) self.wait_duration = wait_duration self.action_topic = action_topic if (self.action_topic is not None) and (len(self.action_topic) > 0): # If topic is defined, set the client up on startup self.client = ProxyActionClient({self.action_topic: ApplyPlanningSceneAction}, self.wait_duration) else: self.action_topic = None # override "" self.moveit_client = None try: self.moveit_client = ProxyMoveItClient(None) except Exception as e: Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e))) self.success = None self.return_code = None
def __init__(self, timeout=5.0, wait_duration=0.001, action_topic=None): ''' Constructor ''' super(QueryPlannersState, self).__init__(input_keys=['action_topic'], outcomes=['done', 'failed'], output_keys=['planner_interfaces']) self.client = None self.timeout_duration = rospy.Duration(timeout) self.wait_duration = wait_duration self.given_action_topic = action_topic if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0): # If topic is defined, set the client up on startup self.client = ProxyActionClient( {self.given_action_topic: QueryPlannersAction}, self.wait_duration) else: self.given_action_topic = None # handle empty string self.current_action_topic = self.given_action_topic self.planner_interfaces = list() self.return_code = None
def __init__(self, gripper_cmd, time=5.0): """Constructor""" super(GripperCommandState, self).__init__(outcomes=['done', 'failed']) self._joint_names = [ 'gripper_finger_joint_l', 'gripper_finger_joint_r' ] self._gripper_joint_positions = { # gripper_finger_joint_l, gripper_finger_joint_r 0: [0.001, 0.001], 1: [0.010, 0.010] #NOTE: 0.011 results in goal tolerance violation } self._gripper_cmd = gripper_cmd self._time = time self._action_topic = "/arm_1/gripper_controller/follow_joint_trajectory" self._client = ProxyActionClient( {self._action_topic: FollowJointTrajectoryAction}) self._done = False self._failed = False
def __init__(self, move_group='whole_body', action_topic = '/move_group', orien_tolerance=True): ''' Constructor ''' super(hsr_MoveitToPoseGoalActionNew, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed'], output_keys=['move_status'], input_keys=['pose_goal','plan_num']) self._action_topic = action_topic self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self.orien_tolerance = orien_tolerance self._move_group = move_group self._planning_failed = False self._control_failed = False self._success = False self.count_num = None self._tolerance = 0.01 robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander(self._move_group) self.planning_frame = group.get_planning_frame() self.planning_frame = '/map' self.eef_link = group.get_end_effector_link() group_names = robot.get_group_names() scene = moveit_commander.PlanningSceneInterface()
def __init__(self, controller='motion/controller/joint_trajectory', outcomes=[ 'success', 'partial_movement', 'invalid_pose', 'failure' ], input_keys=[], *args, **kwargs): # Declare outcomes and output keys super(ExecuteJointTrajectoryBase, self).__init__(outcomes=outcomes, input_keys=input_keys, *args, **kwargs) # Connect to action server. self._controller = controller self._client = ProxyActionClient({ self._controller: FollowJointTrajectoryAction }) # pass required clients as dict (topic: type) # It may happen that the action client fails to send the action goal, if _outcome is set then execute() exits immediately. self._outcome = None # log_once support self._logged = False self._logfunc = { 'info': Logger.loginfo, 'warn': Logger.logwarn, 'err': Logger.logerr, 'hint': Logger.loghint }
def __init__(self, distance=1, ReplanPeriod=0.5): """Constructor""" super(SaraFollow, self).__init__(outcomes=['failed'], input_keys=['ID']) # Prepare the action client for move_base self._action_topic = "/move_base" self._client = ProxyActionClient({self._action_topic: MoveBaseAction}) # Initialise the subscribers self.entities_topic = "/entities" self._pose_topic = "/robot_pose" self._sub = ProxySubscriberCached({ self._pose_topic: Pose, self.entities_topic: Entities }) # Get the parameters self.targetDistance = distance self.ReplanPeriod = ReplanPeriod # Initialise the status variables self.MyPose = None self.lastPlanTime = 0
def __init__(self, topic): """Constructor""" super(BigRollState, self).__init__(outcomes=['Success', 'Failure'], input_keys=['angle']) self._topic = topic self.client = ProxyActionClient( {self._topic: riptide_controllers.msg.GoToRollAction})
def __init__(self, topic, x, y): """Constructor""" super(BigTranslateParameterState, self).__init__(outcomes=['Success', 'Failure']) self._topic = topic self._x = x self._y = y self.client = ProxyActionClient({self._topic: riptide_controllers.msg.MoveDistanceAction})
def __init__(self, config_name, move_group, action_topic = '/move_group', robot_name = ""): ''' Constructor ''' super(SrdfStateToMoveit, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed', 'param_error']) self._config_name = config_name self._move_group = move_group self._robot_name = robot_name self._action_topic = action_topic self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._planning_failed = False self._control_failed = False self._success = False self._srdf_param = None if rospy.has_param("/robot_description_semantic"): self._srdf_param = rospy.get_param("/robot_description_semantic") else: Logger.logerror('Unable to get parameter: /robot_description_semantic') self._param_error = False self._srdf = None
def __init__(self, move_group, offset, tool_link, action_topic='/move_group'): ''' Constructor ''' super(MoveitToJointsDynState, self).__init__( outcomes=['reached', 'planning_failed', 'control_failed'], input_keys=['joint_values', 'joint_names']) self._offset = offset self._tool_link = tool_link self._action_topic = action_topic self._fk_srv_topic = '/compute_fk' self._cartesian_srv_topic = '/compute_cartesian_path' self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) self._fk_srv = ProxyServiceCaller({self._fk_srv_topic: GetPositionFK}) self._cartesian_srv = ProxyServiceCaller( {self._cartesian_srv_topic: GetCartesianPath}) self._current_group_name = move_group self._joint_names = None self._planning_failed = False self._control_failed = False self._success = False self._traj_client = actionlib.SimpleActionClient( 'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction) self._traj_client.wait_for_server() self._traj_exec_result = None
def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False self._config_name = userdata.config_name # Currently not used self._robot_name = userdata.robot_name # Currently not used self._move_group = userdata.move_group self._action_topic = userdata.action_topic self._joint_config = userdata.joint_values self._joint_names = userdata.joint_names self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) # Action Initialization action_goal = MoveGroupGoal() action_goal.request.group_name = self._move_group action_goal.request.allowed_planning_time = 1.0 goal_constraints = Constraints() for i in range(len(self._joint_names)): goal_constraints.joint_constraints.append( JointConstraint(joint_name=self._joint_names[i], position=self._joint_config[i], weight=1.0)) action_goal.request.goal_constraints.append(goal_constraints) try: self._client.send_goal(self._action_topic, action_goal) userdata.action_topic = self._action_topic # Save action topic to output key except Exception as e: Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True
def __init__(self, controller='motion/controller/joint_state_head', operational=True, resources=[], sync=True): super(SetOperational, self).__init__(outcomes=['done', 'failure']) # Store topic parameter for later use. self._controller = controller self._operational = operational self._sync = sync self._resources = resources # create proxies self._action_client = ProxyActionClient( {self._controller: SetOperationalAction}) # check parameters if not isinstance(resources, list) or not all( [isinstance(r, str) for r in resources]): raise ValueError( 'SetOperational: resource must be list of strings') if not isinstance(operational, bool) or not isinstance(sync, bool): raise ValueError( 'SetOperational: sync and operational parameters must be bool.' ) # error in enter hook self._error = False
def __init__(self, components=1023, timeout=5.0, wait_duration=2.0, action_topic=None): ''' Constructor ''' super(GetPlanningSceneState, self).__init__( input_keys=['action_topic'], outcomes=['done', 'failed'], output_keys=['scene'] ) self.client = None self.components = components self.timeout_duration = rospy.Duration(timeout) self.wait_duration = wait_duration self.given_action_topic = action_topic if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0): # If topic is defined, set the client up on startup self.client = ProxyActionClient({self.given_action_topic: GetPlanningSceneAction}, self.wait_duration) else: self.given_action_topic = None self.current_action_topic = self.given_action_topic self.scene = list() self.return_code = None
def on_enter(self, userdata): self._planning_failed = False self._control_failed = False self._success = False self._joint_config = userdata.joint_values self._joint_names = userdata.joint_names self._sub = ProxySubscriberCached({self._position_topic: JointState}) self._current_position = [] self._client = ProxyActionClient({self._action_topic: MoveGroupAction}) # Action Initialization action_goal = MoveGroupGoal() action_goal.request.group_name = self._move_group action_goal.request.allowed_planning_time = 1.0 goal_constraints = Constraints() for i in range(len(self._joint_names)): goal_constraints.joint_constraints.append( JointConstraint(joint_name=self._joint_names[i], position=self._joint_config[i], weight=1.0)) action_goal.request.goal_constraints.append(goal_constraints) try: self._client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e))) self._planning_failed = True
def __init__( self, output_topic="/bounding_box_2d_monitor", rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color", depth_topic="/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw", camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info", yolo_yaml="yolov3.yaml", save_image=True, z_offset=0.1, MEMO="/spacoty/get_dataset"): super(hsr_SpaCoTyGetDataset, self).__init__(outcomes=['completed', 'failed'], input_keys=['save_folder']) # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = { "img": "bounding_box_2d", "sw": "/spacoty/get_dataset", "wrd": "/spacoty/place_name" } self._output_topic = output_topic self._depth_topic = depth_topic self._rgb_topic = rgb_topic self._camera_info_topic = camera_info_topic self._save_folder = "/root/HSR/catkin_ws/src/em_spco_tidy_up/training_data/" self._save_image = save_image self.camera_info = None self.rgb_image = None self.depth_image = None self.z_offset = z_offset # get object labels # self._yolo_yaml_path = "/root/HSR/catkin_ws/src/hsr_launch/config/darknet_ros/"+yolo_yaml # obj_yaml = rosparam.load_file(self._yolo_yaml_path) # self.obj_class = obj_yaml[0][0]["yolo_model"]["detection_classes"]["names"] self._yolo_yaml_path = "/root/HSR/catkin_ws/src/cv_detect_object/scripts/" + yolo_yaml obj_yaml = rosparam.load_file(self._yolo_yaml_path) self.obj_class = obj_yaml[0][0].values() # pass required clients as dict (topic: type) self._client = ProxyActionClient( {self._topic["img"]: hsr_common.msg.BoundingBox2DAction}) self.bridge = cv_bridge.CvBridge() # Subscriber self.sub_sw = ProxySubscriberCached({self._topic["sw"]: Bool}) self.sub_word = ProxySubscriberCached({self._topic["wrd"]: String}) # define the dimension self._data_dim = {"pose": 3, "object": len(self.obj_class), "word": 0} # It may happen that the action client fails to send the action goal. self._error = False self._get_image_error = False
def __init__(self, topic, depth): """Constructor""" super(BigDepthParameterState, self).__init__(outcomes=['Success', 'Failure']) self._topic = topic self._depth = depth self.client = ProxyActionClient( {self._topic: riptide_controllers.msg.GoToDepthAction})
def __init__(self, topic): """Constructor""" super(BigTranslateState, self).__init__(outcomes=['Success', 'Failure'], input_keys=['x', 'y']) self._topic = topic self.client = ProxyActionClient( {self._topic: riptide_controllers.msg.MoveDistanceAction})
def __init__(self, topic, angle): """Constructor""" super(BigRollParameterState, self).__init__(outcomes=['Success', 'Failure']) self._topic = topic self._angle = angle self.client = ProxyActionClient( {self._topic: riptide_controllers.msg.GoToRollAction})