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._failed = False
		self._done = False

		# Create footstep planner request
		strafing = self._direction == PatternParameters.STRAFE_LEFT or self._direction == PatternParameters.STRAFE_RIGHT
		pattern_parameters = PatternParameters()
		pattern_parameters.mode = self._direction
		pattern_parameters.step_distance_forward = self._step_distance_forward if not strafing else 0.0 # will it ignore?
		pattern_parameters.step_distance_sideward = self._step_distance_sideward if strafing else 0.0 # will it ignore?
		pattern_parameters.close_step = True
		step_distance = pattern_parameters.step_distance_sideward if strafing else pattern_parameters.step_distance_forward
		pattern_parameters.steps = int(round(userdata.distance / step_distance))

		request = StepPlanRequest()
		request.parameter_set_name = String('drc_step_no_collision')
		request.header = Header(frame_id = '/world', stamp = rospy.Time.now())
		request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
		request.pattern_parameters = pattern_parameters

		action_goal = StepPlanRequestGoal(plan_request = request)

		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Was unable to create footstep pattern for wide stance:\n%s' % str(e))
			self._failed = True
	def on_enter(self, userdata):
		'''Upon entering, create a neck trajectory and send the action goal.'''

		self._failed = False

		# Create neck point
		neck_point = JointTrajectoryPoint()
		neck_point.time_from_start = rospy.Duration.from_sec(1.0)
		neck_point.positions.append(self._desired_tilt)

		# Create neck trajectory
		neck_trajectory = JointTrajectory()
		neck_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
		neck_trajectory.joint_names = ['neck_ry']
		neck_trajectory.points.append(neck_point)

		# Create action goal
		action_goal = FollowJointTrajectoryGoal(trajectory = neck_trajectory)

		# execute the motion
		try: 
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to send neck trajectory goal:\n%s' % str(e))
			self._failed = True
	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 on_enter(self, userdata):
		self._sampling_failed = False
		self._planning_failed = False
		self._control_failed = False
		self._success = False

		action_goal = ArgoCombinedPlanGoal()
		action_goal.target = deepcopy(userdata.object_pose)
		
		type_map = {
			'dial_gauge': ObjectTypes.DIAL_GAUGE,
			'level_gauge': ObjectTypes.LEVEL_GAUGE,
			'valve': ObjectTypes.VALVE,
			'hotspot': ObjectTypes.HOTSPOT,
			'pipes' : ObjectTypes.PIPES,
			'victim' : ObjectTypes.VICTIM
		}
		object_type = type_map.get(userdata.object_type, ObjectTypes.UNKNOWN)
		
		q = [ action_goal.target.pose.orientation.x, action_goal.target.pose.orientation.y,
			  action_goal.target.pose.orientation.z, action_goal.target.pose.orientation.w ]
		q = tf.transformations.quaternion_multiply(q, self._rot)
		action_goal.target.pose.orientation = Quaternion(*q)
		
		action_goal.object_id.data = userdata.object_id
		action_goal.object_type.val = object_type
		action_goal.action_type.val = ActionCodes.SAMPLE_MOVE_ARM

		Logger.loginfo('Position arm to look at %s object' % str(userdata.object_type))
		
		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to send move group goal for arm motion:\n%s' % str(e))
			self._control_failed = True
	def execute(self, userdata):
		'''
		Execute this state
		'''
		if self._planning_failed:
			return 'planning_failed'
		if self._control_failed:
			return 'control_failed'
		if self._success:
			return 'reached'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			
			if result.error_code.val == MoveItErrorCodes.CONTROL_FAILED:
				Logger.logwarn('Control failed for move action of group: %s (error code: %s)' % (self._move_group, str(result.error_code)))
				self._control_failed = True
				return 'control_failed'
			elif result.error_code.val != MoveItErrorCodes.SUCCESS:
				Logger.logwarn('Move action failed with result error code: %s' % str(result.error_code))
				self._planning_failed = True
				return 'planning_failed'
			else:
				self._success = True
				return 'reached'
    def on_enter(self, userdata):
        """On enter, create and send the follow joint trajectory action goal."""

        self._failed = False

        current_config = userdata.current_config

        # Get the index of the joint whose value will be replaced
        index_of_joint = current_config["joint_names"].index(self._joint_name)

        # Replace the old joint value with the target_config's one
        new_values = current_config["joint_values"]
        new_values[index_of_joint] = self._joint_value

        # Create trajectory point out of the revised joint values
        point = JointTrajectoryPoint(positions=new_values, time_from_start=rospy.Duration.from_sec(self._time))

        # Create trajectory message
        trajectory = JointTrajectory(joint_names=current_config["joint_names"], points=[point])

        action_goal = FollowJointTrajectoryGoal(trajectory=trajectory)

        # execute the motion
        try:
            self._client.send_goal(self._action_topic, action_goal)
        except Exception as e:
            Logger.logwarn("Failed to send trajectory action goal:\n%s" % str(e))
            self._failed = True
	def on_exit(self, userdata):
		# Make sure that the action is not running when leaving this state.
		# A situation where the action would still be active is for example when the operator manually triggers an outcome.

		if not self._client.has_result(self._topic):
			self._client.cancel(self._topic)
			Logger.loginfo('Cancelled active action goal.')
	def on_enter(self, userdata):

		self._startTime = Time.now()

		self._failed = False
		self._reached = False
		
		goal_id = GoalID()
		goal_id.id = 'abcd'
		goal_id.stamp = Time.now()

		action_goal = MoveBaseGoal()
		action_goal.target_pose = userdata.waypoint
		action_goal.speed = userdata.speed

		if action_goal.target_pose.header.frame_id == "":
			action_goal.target_pose.header.frame_id = "world"

		try:
			self._move_client.send_goal(self._action_topic, action_goal)
			resp = self.set_tolerance(goal_id, 0.2, 1.55)
		except Exception as e:
			Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % {
				'err': str(e),
				'x': userdata.waypoint.pose.position.x,
				'y': userdata.waypoint.pose.position.y
			})
			self._failed = True
		
		Logger.loginfo('Driving to next waypoint')
	def combine_plans(self, plans):
		output_dict = {}
		try:
			left_arm = plans[0]
			right_arm = plans[1]
			left_leg = plans[2]
			right_leg = plans[3]
			torso = plans[4]
			#hacky_dict_combination = lambda p: dict({'left_arm': p[0]}, **{'right_arm': p[1]})
	
			# Logger.loginfo('SystemIdentificationTestsSM:combine_plans: plans="%s"' % str(plans))
	
			if left_arm is not None:
				output_dict['left_arm'] = left_arm.joint_trajectory
	
			if right_arm is not None: 
				output_dict['right_arm'] = right_arm.joint_trajectory
	
			if left_leg is not None:
				output_dict['left_leg'] = left_leg.joint_trajectory
	
			if right_leg is not None: 
				output_dict['right_leg'] = right_leg.joint_trajectory
	
			if torso is not None: 
				output_dict['torso'] = torso.joint_trajectory
		except Exception as e:
			Logger.loginfo('SystemIdentificationTestsSM:combine_plans: Error "%s"' % (str(e)))
		return output_dict
示例#11
0
    def on_enter(self, userdata):
        # When entering this state, we send the action goal once to let the robot start its work.

        # As documented above, we get the specification of which dishwasher to use as input key.
        # This enables a previous state to make this decision during runtime and provide the ID as its own output key.

        # Create the goal.
        tweet = SendTweetGoal() 
        tweet.text = userdata.tweet_text
        
        if len(userdata.tweet_text) > 140:
            tweet.text = '#LAMoR15 #ECMR15 I just told a too looong joke, stupid tweet length!'

        tweet.with_photo = True
        img = cv2.imread(userdata.picture_path)

        bridge = CvBridge()
        image_message = bridge.cv2_to_imgmsg(img, encoding="bgr8")
        tweet.photo = image_message

        try:
            self._client.send_goal(self._topic, tweet)
        except Exception as e:
                # Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
                # Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
            Logger.logwarn('Failed to send the TweetPictureState command:\n%s' % str(e))
            self._error = True
示例#12
0
	def on_enter(self, userdata):
			
		speedValue = self._dynrec.get_configuration(timeout = 0.5)
		if speedValue is not None:
			self._defaultspeed = speedValue['speed']	
			
		self._dynrec.update_configuration({'speed':userdata.speed})		
		self._succeeded = False
		self._failed = False
		
		action_goal = MoveBaseGoal()
		action_goal.exploration = True
		action_goal.speed = userdata.speed

		if action_goal.target_pose.header.frame_id == "":
			action_goal.target_pose.header.frame_id = "world"

		try:
			if self._move_client.is_active(self._action_topic):
				self._move_client.cancel(self._action_topic)
			self._move_client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Failed to start Exploration' % {
				'err': str(e),
				'x': userdata.waypoint.pose.position.x,
				'y': userdata.waypoint.pose.position.y
			})
			self._failed = True
	def set_back_rotation(self, input_keys):
		affordance = input_keys[0]
		back_rotation = input_keys[1]
		degrees = math.copysign(back_rotation, affordance.displacement)
		affordance.displacement = math.radians(degrees)
		Logger.loginfo("Turning back %.1f degrees" % degrees)
		return affordance
	def __init__(self, config_name, srdf_file, move_group = "", robot_name = ""):
		'''
		Constructor
		'''
		super(GetJointsFromSrdfState, self).__init__(outcomes=['retrieved', 'file_error'],
												output_keys=['joint_values'])

		self._config_name = config_name
		self._move_group = move_group
		self._robot_name = robot_name

		# Check existence of SRDF file to reduce risk of runtime failure.
		# Anyways, values will only be read during runtime to allow modifications.
		self._srdf_path = None
		try:
			rp = RosPack()
			pkg_name = srdf_file.split('/')[0]
			self._srdf_path = os.path.join(rp.get_path(pkg_name), '/'.join(srdf_file.split('/')[1:]))
			if not os.path.isfile(self._srdf_path):
				raise IOError('File "%s" does not exist!' % self._srdf_path)
		except Exception as e:
			Logger.logwarn('Unable to find given SRDF file: %s\n%s' % (srdf_file, str(e)))

		self._file_error = False
		self._srdf = None
	def execute(self, userdata):
		'''
		Execute this state
		'''
		if not self._connected:
			return 'no_connection'
		if self._received:
			return 'received'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			if result.result_code != BehaviorInputResult.RESULT_OK:
				userdata.data = None
				return 'aborted'
			else:
				try:
					response_data = pickle.loads(result.data)
					#print 'Input request response:', response_data
					userdata.data = response_data
					# If this was a template ID request, log that template ID:
					if self._request == BehaviorInputGoal.SELECTED_OBJECT_ID:
						Logger.loginfo('Received template ID: %d' % int(response_data))
				
				except Exception as e:
					Logger.logwarn('Was unable to load provided data:\n%s' % str(e))
					userdata.data = None
					return 'data_error'

				self._received = True
				return 'received'
	def _version_callback(self, msg):
		vui = self._parse_version(msg.data)
		vex = self._parse_version(BehaviorLauncher.MIN_VERSION)
		if vui < vex:
			Logger.logwarn('FlexBE App needs to be updated!\n' \
				+ 'Require at least version %s, but have %s\n' % (BehaviorLauncher.MIN_VERSION, msg.data) \
				+ 'You can update the app by dropping the file flexbe_behavior_engine/FlexBE.crx onto the Chrome extension page.')
    def __init__(self, config_name, move_group="", duration=1.0, wait_for_execution=True, action_topic='/execute_kinematic_path', robot_name=""):
        '''
                Constructor
                '''
        super(SrdfStateToMoveitExecute, self).__init__(
            outcomes=['reached', 'request_failed', 'moveit_failed', 'param_error'])

        self._config_name  = config_name
        self._robot_name   = robot_name
        self._move_group   = move_group
        self._duration     = duration
        self._wait_for_execution = wait_for_execution
        self._action_topic = action_topic
        self._client       = ProxyServiceCaller({self._action_topic: ExecuteKnownTrajectory})

        # self._action_topic = action_topic
        # self._client       =  ProxyActionServer({self._action_topic: ExecuteTrajectoryAction})

        self._request_failed = False
        self._moveit_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
        self._response = None
	def on_enter(self, userdata):
		"""Upon entering the state"""

		try:
			self._pub.publish(self._topic, OCSLogging(run=self._command, no_video=self._no_video, no_bags=self._no_bags, experiment_name=userdata.experiment_name, description=userdata.description))
		except Exception as e:
			Logger.logwarn('Could not send video logging command:\n %s' % str(e))
	def __init__(self, controllers = ["left_arm_traj_controller", "right_arm_traj_controller"]):
		'''Constructor'''
		
		super(ExecuteTrajectoryBothArmsState, self).__init__(outcomes = ['done', 'failed'],
															 input_keys = ['trajectories'])

		self._controllers = controllers
		self._controllers = ["left_arm_traj_controller", "right_arm_traj_controller"] if not(controllers) else controllers

		self._client = ProxyActionClient()
		
		self._client_topics = dict()
		self._active_topics = list()
		
		for controller in self._controllers:
			if "left_arm" in controller:
				action_topic_left  = "/trajectory_controllers/" + controller + "/follow_joint_trajectory"
				self._client.setupClient(action_topic_left, FollowJointTrajectoryAction)
				self._client_topics['left_arm'] = action_topic_left
			elif "right_arm" in controller:
				action_topic_right = "/trajectory_controllers/" + controller + "/follow_joint_trajectory"
				self._client.setupClient(action_topic_right, FollowJointTrajectoryAction)
				self._client_topics['right_arm'] = action_topic_right
			else:
				Logger.logwarn('The controller is neither a left nor a right arm trajectory controller!? %s' % str(controller))

		self._failed = False
        def on_enter(self, userdata):
                self._param_error     = False
                self._planning_failed = False
                self._control_failed  = False
                self._success         = False

                #Parameter check
                if self._srdf_param is None:
                        self._param_error = True
                        return

                try:
                        self._srdf = ET.fromstring(self._srdf_param)
                except Exception as e:
                        Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
                        self._param_error = True

                if not self._param_error:

                        robot = None
                        for r in self._srdf.iter('robot'):
                                if self._robot_name == '' or self._robot_name == r.attrib['name']:
                                        robot = r
                                        break
                        if robot is None:
                                Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
                                return 'param_error'

                        config = None
                        for c in robot.iter('group_state'):
                                if (self._move_group == '' or self._move_group == c.attrib['group']) \
                                and c.attrib['name'] == self._config_name:
                                        config = c
                                        self._move_group = c.attrib['group'] #Set move group name in case it was not defined
                                        break
                        if config is None:
                                Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name)
                                return 'param_error'

                        try:
                                self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')]
                                self._joint_names  = [str(j.attrib['name']) for j in config.iter('joint')]
                        except Exception as e:
                                Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
                                return 'param_error'


                        #Action Initialization
                        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=self._joint_config[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, 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 on_enter(self, userdata):
		self._failed = False

		planning_group = ""
		joint_values = []
		if userdata.side == 'left' and self._target_pose in self._poses[self._robot]['left'].keys():
			planning_group = self._poses[self._robot]['left'][self._target_pose]['group']
			joint_values = self._poses[self._robot]['left'][self._target_pose]['joints']
		elif userdata.side == 'right' and self._target_pose in self._poses[self._robot]['right'].keys():
			planning_group = self._poses[self._robot]['right'][self._target_pose]['group']
			joint_values = self._poses[self._robot]['right'][self._target_pose]['joints']
		elif self._target_pose in self._poses[self._robot]['same'].keys():
			planning_group = self._poses[self._robot]['same'][self._target_pose]['group']
			joint_values = self._poses[self._robot]['same'][self._target_pose]['joints']
		else:
			Logger.logwarn('Specified target pose %d is not specified for %s side' % (self._target_pose, userdata.side))
			self._failed = True
			return

		# create the motion goal
		self._client.new_goal(planning_group)
		self._client.add_joint_values(joint_values)
		self._client.set_velocity_scaling(self._vel_scaling)
		self._client.set_collision_avoidance(self._ignore_collisions)
		self._client.add_link_padding(link_paddings = self._link_paddings) #dict
		if self._is_cartesian:
			self._client.set_cartesian_motion()

		# execute the motion
		try: 
			self._client.start_execution()
		except Exception as e:
			Logger.logwarn('Was unable to execute move group request:\n%s' % str(e))
			self._failed = True
    def execute(self, userdata):
        # execute all active states
        for name, state in self._states.items():
            if self._returned_outcomes.has_key(name): continue
            outcome = None
            if isinstance(state, smach.StateMachine):
                rmp_ud = smach.Remapper(
                            userdata,
                            state.get_registered_input_keys(),
                            state.get_registered_output_keys(),
                            {key: name + '_' + key for key in state.get_registered_input_keys() + state.get_registered_output_keys()})
                state.userdata.merge(rmp_ud, rmp_ud.keys(), dict())
                with state._state_transitioning_lock:
                    outcome = state._update_once()
            else:
                outcome = state.execute(smach.Remapper(
                            userdata,
                            state.get_registered_input_keys(),
                            state.get_registered_output_keys(),
                            {key: name + '_' + key for key in state.get_registered_input_keys() + state.get_registered_output_keys()}))
            if outcome is not None and outcome != "loopback":
                Logger.loginfo("Concurrency: %s > %s" % (name, outcome))
                self._returned_outcomes[name] = outcome

        # determine outcome
        for element in self._outcome_mapping:
            state_outcome = element['outcome']
            mapping = element['condition']
            if all(name in self._returned_outcomes and self._returned_outcomes[name] is outcome for name, outcome in mapping.items()):
                return state_outcome
	def on_enter(self, userdata):
		self._failed = False
		request = GetCartesianPathRequest()
		request.group_name = self._move_group
		request.avoid_collisions = not self._ignore_collisions
		request.max_step = 0.05

		request.header = userdata.eef_pose.header
		request.waypoints.append(userdata.eef_pose.pose)

		now = rospy.Time.now()

		try:
			self._tf.waitForTransform('base_link', 'gripper_cam_link', now, rospy.Duration(1))
			(p,q) = self._tf.lookupTransform('gripper_cam_link', 'base_link', now)

			c = OrientationConstraint()
			c.header.frame_id = 'base_link'
			c.header.stamp = now
			c.link_name = 'gripper_cam_link'
			c.orientation.x = q[0]
			c.orientation.y = q[1]
			c.orientation.z = q[2]
			c.orientation.w = q[3]
			c.weight = 1
			c.absolute_x_axis_tolerance = 0.1
			c.absolute_y_axis_tolerance = 0.1
			c.absolute_z_axis_tolerance = 0.1

			#request.path_constraints.orientation_constraints.append(c)

			self._result = self._srv.call(self._topic, request)
		except Exception as e:
			Logger.logwarn('Exception while calculating path:\n%s' % str(e))
			self._failed = True
	def execute(self, userdata):

		if self._failed or self._srv_result is None:
			return 'failed'
		
		if self._done:
			return 'done'

		if self._not_available:
			return 'not_available'

		try:
			choices = self._srv_result.template_type_information.stand_poses
			Logger.loginfo("There are %d stand pose choices and want to select entry %d" % (len(choices), userdata.preference))
			if len(choices) <= userdata.preference:
				self._not_available = True
				return 'not_available'
			userdata.stand_pose = choices[userdata.preference].pose
		except Exception as e:
			Logger.logwarn('Failed to retrieve pose information from service result:\n%s' % str(e))
			self._failed = True
			return 'failed'
		
		self._done = True
		return 'done'
	def __init__(self):
		self._sub = rospy.Subscriber("flexbe/request_behavior", BehaviorRequest, self._callback)
		self._version_sub = rospy.Subscriber("flexbe/ui_version", String, self._version_callback)

		self._pub = rospy.Publisher("flexbe/start_behavior", BehaviorSelection, queue_size=100)
		self._status_pub = rospy.Publisher("flexbe/status", BEStatus, queue_size=100)
		self._mirror_pub = rospy.Publisher("flexbe/mirror/structure", ContainerStructure, queue_size=100)

		self._rp = RosPack()
		self._behavior_lib = dict()

		Logger.initialize()

		behaviors_package = "flexbe_behaviors"
		if rospy.has_param("behaviors_package"):
			behaviors_package = rospy.get_param("behaviors_package")
		else:
			rospy.loginfo("Using default behaviors package: %s" % behaviors_package)

		manifest_folder = os.path.join(self._rp.get_path(behaviors_package), 'behaviors/')

		file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')]
		manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)])

		for i in range(len(manifests)):
			m = ET.parse(manifests[i]).getroot()
			e = m.find("executable")
			self._behavior_lib[i] = {
				"name": m.get("name"),
				"package": e.get("package_path").split(".")[0],
				"file": e.get("package_path").split(".")[1],
				"class": e.get("class")
			}

		rospy.loginfo("%d behaviors available, ready for start request." % len(self._behavior_lib.items()))
	def on_enter(self, userdata):
		''' 
		Sending plan request with wide stance mode
		'''

		self._failed = False
		self._done = False

		pattern_parameters = PatternParameters()
		pattern_parameters.steps = 4
		pattern_parameters.mode = PatternParameters.FEET_REALIGN_ON_CENTER
		pattern_parameters.close_step = True
		pattern_parameters.extra_seperation = False

		request = StepPlanRequest()
		request.parameter_set_name = String('drc_step_no_collision')
		request.header = Header(frame_id = '/world', stamp = rospy.Time.now())
		request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
		request.pattern_parameters = pattern_parameters

		action_goal = StepPlanRequestGoal(plan_request = request)

		try:
			self._client.send_goal(self._action_topic, action_goal)
		except Exception as e:
			Logger.logwarn('Was unable to create footstep pattern for wide stance:\n%s' % str(e))
			self._failed = True
	def set_retract_distance(self, affordance):
		'''Sets the displacement of the "insert" affordance to a negative number.'''

		retract_distance = -abs(self._insert_distance) # negative number means retract
		affordance.displacement = retract_distance
		Logger.loginfo("Will be retracting by %d centimeters" % (retract_distance*100))
		
		return affordance
	def on_enter(self, userdata):

		try:
			self._srv_result = self._srv.call(self._service_topic, GetTemplateStateAndTypeInfoRequest(userdata.template_id, 0)) # We don't care about the hand side
		
		except Exception as e:
			Logger.logwarn('Failed to send service call:\n%s' % str(e))
			self._failed = True
	def set_push_distance(self, affordance):
		'''Sets the displacement of the "insert" affordance to a positive number.'''

		push_distance = +abs(self._push_distance)
		affordance.displacement = push_distance
		Logger.loginfo("Will be retracting by %d centimeters" % (push_distance*100))
		
		return affordance
    def execute(self, userdata):
        if self._bumper_sub.has_msg(self._bumper_topic):
            sensor = self._bumper_sub.get_last_msg(self._bumper_topic)
            self._bumper_sub.remove_last_msg(self._bumper_topic)

            if sensor.state > 0:
                Logger.logwarn('Bumper %d contact' % (sensor.bumper))
                self._return = 'bumper'

        if self._cliff_sub.has_msg(self._cliff_topic):
            sensor = self._cliff_sub.get_last_msg(self._cliff_topic)
            self._cliff_sub.remove_last_msg(self._cliff_topic)
            if sensor.state > 0:
                Logger.logwarn('Cliff alert')
                self._return = 'cliff'

        return self._return
    def check_plan(self, pose, orientation=False):
        action_goal = make_default_action_goal(self, pose, orientation)
        action_goal.planning_options.planning_scene_diff.is_diff = True
        action_goal.planning_options.planning_scene_diff.robot_state.is_diff = True
        action_goal.planning_options.plan_only = True
        try:
            self._client.send_goal(action_goal)
            self._client.wait_for_result()
            res = self._client.get_result()
            print(res.error_code.val, self.planning_frame)
            if res.error_code.val < 1:
                return False
            else:
                return True

        except Exception as e:
            Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
    def on_enter(self, userdata):
        self._failed = False

        # create the motion goal
        self._client.new_goal(self._planning_group)
        self._client.add_joint_values(userdata.target_joint_config)

        # execute the motion
        try:
            Logger.loginfo("Moving %s to: %s" %
                           (self._planning_group, ", ".join(
                               map(str, userdata.target_joint_config))))
            self._client.start_execution()
        except Exception as e:
            Logger.logwarn('Was unable to execute move group request:\n%s' %
                           str(e))
            self._failed = True
	def execute(self, userdata):
		if self._failed:
			return 'failed'

		if self._client.has_result(self._action_topic):
			result = self._client.get_result(self._action_topic)
			if result:
				if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
					return 'done'
				else:
					Logger.logwarn('Joint trajectory request failed to execute (%d). Reason: %s' % (result.error_code, result.error_string))
					self._failed = True
					return 'failed'
			else:
				Logger.logwarn('Wait for result returned True even though the result is %s' % str(result))
				self._failed = True
				return 'failed'
示例#35
0
    def execute(self, userdata):
        # While this state is active, check if the action has been finished and evaluate the result.

        # Check if the client failed to send the goal.
        if self._error:
            return 'failed'

        # Check if the action has been finished
        if self._client.has_result(self._topic):
            result = self._client.get_result(self._topic)

            # Set output keys
            userdata.outcome = {
                'origin': 'get_path',
                'outcome': result.outcome
            }
            userdata.path = result.path

            # Send result log
            Logger.loginfo(result.message)

            # Based on the result, decide which outcome to trigger.
            if result.outcome == GetPathResult.SUCCESS:
                return 'succeeded'
            elif result.outcome == GetPathResult.FAILURE:
                return 'failed'
            elif result.outcome == GetPathResult.NO_PATH_FOUND:
                return 'failed'
            elif result.outcome == GetPathResult.INVALID_GOAL:
                return 'failed'
            elif result.outcome == GetPathResult.INVALID_PLUGIN:
                return 'aborted'
            elif result.outcome == GetPathResult.INVALID_START:
                return 'aborted'
            elif result.outcome == GetPathResult.EMPTY_PATH:
                return 'aborted'
            elif result.outcome == GetPathResult.INTERNAL_ERROR:
                return 'aborted'
            elif result.outcome == GetPathResult.NOT_INITIALIZED:
                return 'aborted'
            elif result.outcome == GetPathResult.PAT_EXCEEDED:
                return 'failed'
            elif result.outcome == GetPathResult.STOPPED:
                return 'aborted'
            else:
                return 'aborted'
示例#36
0
    def __init__(self):
        '''
        Constructor
        '''
        self.be = None
        Logger.initialize()
        smach.set_loggers(
            rospy.logdebug,  # hide SMACH transition log spamming
            rospy.logwarn,
            rospy.logdebug,
            rospy.logerr)

        #ProxyPublisher._simulate_delay = True
        #ProxySubscriberCached._simulate_delay = True

        # prepare temp folder
        rp = rospkg.RosPack()
        self._tmp_folder = tempfile.mkdtemp()
        sys.path.append(self._tmp_folder)
        rospy.on_shutdown(self._cleanup_tempdir)

        # prepare manifest folder access
        self._behavior_lib = BehaviorLibrary()

        self._pub = ProxyPublisher()
        self._sub = ProxySubscriberCached()

        self.status_topic = 'flexbe/status'
        self.feedback_topic = 'flexbe/command_feedback'

        self._pub.createPublisher(self.status_topic, BEStatus, _latch=True)
        self._pub.createPublisher(self.feedback_topic, CommandFeedback)

        # listen for new behavior to start
        self._running = False
        self._switching = False
        self._sub.subscribe('flexbe/start_behavior', BehaviorSelection,
                            self._behavior_callback)

        # heartbeat
        self._pub.createPublisher('flexbe/heartbeat', Empty)
        self._execute_heartbeat()

        rospy.sleep(0.5)  # wait for publishers etc to really be set up
        self._pub.publish(self.status_topic, BEStatus(code=BEStatus.READY))
        rospy.loginfo('\033[92m--- Behavior Engine ready! ---\033[0m')
 def _convert_input_data(self, keys, values):
     result = dict()
     for k, v in zip(keys, values):
         # action call has empty string as default, not a valid input key
         if k == '':
             continue
         try:
             result[k] = self._convert_dict(cast(v))
         except ValueError:
             # unquoted strings will raise a ValueError, so leave it as string in this case
             result[k] = str(v)
         except SyntaxError as se:
             Logger.loginfo(
                 'Unable to parse input value for key "%s", assuming string:\n%s\n%s'
                 % (k, str(v), str(se)))
             result[k] = str(v)
     return result
	def execute(self, userdata):
		'''Execute this state'''
		if self._subscriber.has_msg(self._topic):
			msg = self._subscriber.get_last_msg(self._topic)
			# in case you want to make sure the same message is not processed twice:
			self._subscriber.remove_last_msg(self._topic)
			if not msg.found:
				return 'success'

			self.calculate_error(msg)
			Logger.loghint(self._error_result)

			for key in self._keys:
				# check tolerance
				if self._tolerance < abs(self._error_result[key]):
					userdata.output_value = self._error_result
					return 'unacceptable'
    def on_enter(self, userdata):
        # This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
        # It is primarily used to start actions which are associated with this state.

        # Get transform between camera and robot1_base
        while True:
            rospy.sleep(0.1)
            try:
                target_pose = self._tf_buffer.transform(userdata.pose, "world")
                break
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                    tf2_ros.ExtrapolationException):
                rospy.logerr(
                    "ComputeGraspState::on_enter - Failed to transform to world"
                )
                continue

        # the grasp pose is defined as being located on top of the box
        target_pose.pose.position.z += self._offset + 0.0

        # rotate the object pose 180 degrees around - now works with -90???

        #q_orig = [target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]
        q_orig = [0, 0, 0, 1]
        q_rot = quaternion_from_euler(self._rotation, 0, 0)
        res_q = quaternion_multiply(q_rot, q_orig)
        target_pose.pose.orientation = geometry_msgs.msg.Quaternion(*res_q)

        # use ik service to compute joint_values
        self._srv_req = GetPositionIKRequest()
        self._srv_req.ik_request.group_name = self._group
        self._srv_req.ik_request.robot_state.joint_state = rospy.wait_for_message(
            self._group + '/joint_states', sensor_msgs.msg.JointState)

        self._srv_req.ik_request.ik_link_name = self._tool_link  # TODO: this needs to be a parameter
        self._srv_req.ik_request.pose_stamped = target_pose
        self._srv_req.ik_request.avoid_collisions = False
        self._srv_req.ik_request.attempts = 500

        try:
            self._srv_result = self._ik_srv.call(self._srv_name, self._srv_req)
            self._failed = False

        except Exception as e:
            Logger.logwarn('Could not call IK: ' + str(e))
            self._failed = True
    def execute(self, userdata):

        if self.preempt_requested():
            self.service_preempt()
            return 'preempted'

        request = GetTaskRequest()
        # before userdata.task_details_task_id
        request.task_id = 'victim_0_autonomous'

        try:
            response = self.getTask.call(self._topic_get_task, request).task
        except Exception, e:
            Logger.loginfo('[behavior_get_closer] Could not get task:' +
                           str(e))
            rospy.sleep(0.5)
            return 'waiting'
    def on_enter(self, userdata):
        # Update self.current_pose.
        whole_body = self.robot.get('whole_body')
        self.current_pose = whole_body.get_end_effector_pose(ref_frame_id='map')
        target_poses = [poses for poses in userdata.target_poses.items()]
        #target_poses = userdata.target_poses
        category = userdata.category
        # failed_objects = userdata.failed_objects
        # collections = {"known": self.known_dict.keys(), "unknown": self.unknown_list}
        candidates = [category]
        print("category: ", candidates)
        target_pose = [pose for pose in target_poses if pose[0] in candidates]
        print("target: ", target_pose)
        # target_pose = [pose for pose in target_pose if not pose[0] in [object for object, value in failed_objects.items() if value is True]]
        if target_pose != []:
            print('target_pose:', target_pose)
            distance_array = np.array([])
            for pose in target_pose:
                distance = \
                math.sqrt(pow((pose[1].position.x - self.current_pose.pos.x), 2) \
                + pow((pose[1].position.y - self.current_pose.pos.y), 2) \
                + pow((pose[1].position.z - self.current_pose.pos.z), 2))
                distance_array = np.append(distance_array, distance)
            distance_index = np.argsort(distance_array)
            self.selected_pose_grasp = copy.deepcopy(target_pose[distance_index[0]][1])
            self.selected_object_name = target_pose[distance_index[0]][0]
            self.selected_object_status = 'Success!'


            fix_orientation =self.quaternion_from_position(self.current_pose.pos, self.selected_pose_grasp.position)
            self.selected_pose_grasp.orientation.x, self.selected_pose_grasp.orientation.y, \
            self.selected_pose_grasp.orientation.z, self.selected_pose_grasp.orientation.w = fix_orientation
            print("grasp: ", self.selected_pose_grasp)
            print("name: ", self.selected_object_name)
            # if self.check_plan(pose = self.selected_pose_grasp, orientation=True) == False:
            #     self.selected_pose_approach = None
            #     self.selected_pose_grasp = None
            #     self.selected_object_status = 'fail_plan'
            print('object_name: ', self.selected_object_name)
            print('object_status: ', self.selected_object_status)
        else:
            Logger.logwarn('detect result is empty')
            self.selected_pose_grasp = None
            self.selected_object_name = None
            self.selected_object_status = 'not_detect'
    def execute(self, userdata):
        '''
        Execute this state
        '''
        if self.return_code is not None:
            # Return any previously assigned return code if we are in pause
            userdata.status_text = self.status_text
            return self.return_code

        if self.client.has_result(self.current_action_topic):
            result = self.client.get_result(self.current_action_topic)

            if (result.valid):
                self.return_code = 'valid'
                self.status_text = 'checked robot state is valid'
            else:
                self.return_code = 'invalid'
                self.status_text = 'checked robot state is not valid! number of contacts = %d' % (
                    len(result.contacts))

            Logger.loginfo(self.status_text)
            userdata.status_text = self.status_text
            return self.return_code

        elif self.client.get_state(
                self.current_action_topic) == GoalStatus.ABORTED:
            self.status_text = "StateValidation - %s request aborted by state validation action server\n %s" % (
                self.name,
                self.action_client.get_goal_status_text(
                    self.current_action_topic))
            self.return_code = 'failed'
            Logger.logerr(self.status_text)
            userdata.status_text = self.status_text
            return self.return_code
        elif self.client.get_state(
                self.current_action_topic) == GoalStatus.REJECTED:
            # No result returned is returned for this action, so go by the client state
            userdata.valid = self.valid
            self.status_text = "StateValidation - %s request rejected by state validation action server" % (
                self.name,
                self.action_client.get_goal_status_text(
                    self.current_action_topic))
            self.return_code = 'failed'
            Logger.logerr(self.status_text)
            userdata.status_text = self.status_text
            return self.return_code
        elif self.timeout_target is not None and rospy.Time.now(
        ) > self.timeout_target:
            self.status_text = "StateValidation - timeout waiting for %s to state validation (%f > %f (%f))" % (
                self.name, rospy.Time.now().to_sec(),
                self.timeout_target.to_sec(), self.timeout_duration.to_sec())
            self.return_code = 'failed'
            Logger.logerr(self.status_text)
            userdata.status_text = self.status_text
            return self.return_code
    def on_enter(self, userdata):
        self._failed = False

        planning_group = ""
        joint_values = []
        if userdata.side == 'left' and self._target_pose in self._poses[
                self._robot]['left'].keys():
            planning_group = self._poses[self._robot]['left'][
                self._target_pose]['group']
            joint_values = self._poses[self._robot]['left'][
                self._target_pose]['joints']
        elif userdata.side == 'right' and self._target_pose in self._poses[
                self._robot]['right'].keys():
            planning_group = self._poses[self._robot]['right'][
                self._target_pose]['group']
            joint_values = self._poses[self._robot]['right'][
                self._target_pose]['joints']
        elif self._target_pose in self._poses[self._robot]['same'].keys():
            planning_group = self._poses[self._robot]['same'][
                self._target_pose]['group']
            joint_values = self._poses[self._robot]['same'][
                self._target_pose]['joints']
        else:
            Logger.logwarn(
                'Specified target pose %d is not specified for %s side' %
                (self._target_pose, userdata.side))
            self._failed = True
            return

        # create the motion goal
        self._client.new_goal(planning_group)
        self._client.add_joint_values(joint_values)
        self._client.set_velocity_scaling(self._vel_scaling)
        self._client.set_collision_avoidance(self._ignore_collisions)
        self._client.add_link_padding(link_paddings=self._link_paddings)  #dict
        if self._is_cartesian:
            self._client.set_cartesian_motion()

        # execute the motion
        try:
            self._client.start_execution()
        except Exception as e:
            Logger.logwarn('Was unable to execute move group request:\n%s' %
                           str(e))
            self._failed = True
    def on_enter(self, userdata):
        self.save_file = userdata.load_file
        if self.tidy_order == []:
            try:
                dict2string_obj_inf = []
                for k, v in userdata.detect_objects.items():
                    pose_txt = str(v.position.x)+","+ \
                               str(v.position.y)+","+ \
                               str(v.position.z)
                    feature = ""
                    for f in userdata.detect_objects_feature[k]:
                        feature += str(f) + ","
                    # rospy.loginfo("\n\n"+str(feature)+str(len(feature))+"\n\n")
                    dict2string_obj_inf.append(k)
                    dict2string_obj_inf.append(pose_txt)
                    dict2string_obj_inf.append(feature)

                #print"SpaCoTyPlanning debug: "+str(dict2string_obj_inf)
                req = self.em_spacoty_tidy_greedy(dict2string_obj_inf,
                                                  self.save_file)
            except rospy.ServiceException, e:
                rospy.loginfo("[Service spacoty/client] call failed: %s", e)
            if req.done == True:
                request_result = req.tidy_object_order_and_pose
                self.low_obj_prob = req.low_obj_prob
                #tidy_order = []
                # string to dict
                self.s2d = {}
                for i in xrange(len(request_result)):
                    if i % 2 == 0:
                        p = Pose()
                        p_and_o = request_result[i + 1].split(",")
                        p.position.x = float(p_and_o[0])
                        p.position.y = float(p_and_o[1])
                        p.position.z = float(p_and_o[2])
                        #p.orientation.w = 1.0

                        self.s2d.update({request_result[i]: p})
                        self.tidy_order.append(request_result[i])

                str_nm = "["
                for i, nm in enumerate(self.tidy_order):
                    str_nm += " " + str(i) + ": " + nm
                str_nm += "]"
                Logger.loginfo("Plan: " + str_nm)
    def __init__(self):
        '''
        Constructor
        '''
        super(GetJointNamesFromMoveGroupState,
              self).__init__(outcomes=['retrieved', 'param_error'],
                             input_keys=['robot_name', 'selected_move_group'],
                             output_keys=['move_group', 'joint_names'])
        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.return_code = None
示例#46
0
    def on_enter(self, userdata):
        # When entering this state, we send the action goal once to let the robot start its work.

        # Create the goal.
        goal = MoveBaseGoal()
        goal.tolerance = self._tolerance
        goal.target_pose = userdata.target_pose
        goal.planner = self._planner
        goal.controller = self._controller
        goal.recovery_behaviors = self._recovery_behaviors

        # Send the goal.
        self._error = False  # make sure to reset the error state since a previous state execution might have failed
        try:
            self._client.send_goal(self._topic, goal)
        except Exception as e:
            Logger.logwarn('Failed to send goal:\n%s' % str(e))
            self._error = True
	def execute(self, userdata):
		'''Execute this state'''

		print("age: " +str(userdata.age))
		print("gender: " +str(userdata.gender))
		Logger.logwarn("age: " +str(userdata.age))
		Logger.logwarn("gender: " +str(userdata.gender))

		print("talking now")

		if(userdata.age>40 and userdata.gender=="Male"):
			os.system("python3 {}catkin_ws/src/robot_voice/src/ohbot_say_function.py {}".format(home,str(7)))
		if(userdata.age<40 and userdata.gender=="Male"):
			os.system("python3 {}catkin_ws/src/robot_voice/src/ohbot_say_function.py {}".format(home,str(8)))
		if(userdata.gender=="Female"):
			os.system("python3 {}catkin_ws/src/robot_voice/src/ohbot_say_function.py {}".format(home,str(9)))

		return self._outcome
示例#48
0
    def execute(self, userdata):
        """Wait for action result and return outcome accordingly"""

        if self._arrived:
            return 'arrived'
        if self._failed:
            return 'failed'

        if self._client.has_result(self._action_topic):
            status = self._client.get_state(self._action_topic)
            if status == GoalStatus.SUCCEEDED:
                self._arrived = True
                return 'arrived'
            elif status in [GoalStatus.PREEMPTED, GoalStatus.REJECTED,
                            GoalStatus.RECALLED, GoalStatus.ABORTED]:
                Logger.logwarn('Navigation failed: %s' % str(status))
                self._failed = True
                return 'failed'
示例#49
0
    def calc_pose(self, input_list):
        pose = input_list[0]
        sdv = input_list[1]

        out_pose = PoseStamped()
        out_pose.pose.position.x = pose.pose.position.x + random.gauss(
            self.waypoint_distance, sdv)
        out_pose.pose.position.y = pose.pose.position.y + random.gauss(0, sdv)
        out_pose.pose.orientation = Quaternion(w=1)
        out_pose.header = Header(frame_id="world")

        print out_pose

        Logger.loginfo(
            "Calculated new positon (%s/%s)." %
            (str(out_pose.pose.position.x), str(out_pose.pose.position.y)))

        return out_pose
    def execute(self, userdata):
        if self.result:
            return self.result

        if self.waitForExecution:
            curState = self.group.get_current_joint_values()
            diff = compareStates(curState, self.endState)
            print("diff=" + str(diff))
            if diff < self.tol or self.timeout + rospy.Duration(
                    self.watchdog) < rospy.Time.now():
                self.count += 1
                if self.count > 3:
                    Logger.loginfo('Target reached :)')
                    return "done"
            else:
                self.count = 0
        else:
            return "done"
    def __init__(self, topic='/do_adaption'):

        super(HandoverAdaptionReset,
              self).__init__(outcomes=['succeeded', 'error'])

        self._topic = topic
        self._command = 0
        self._reality_damp = 0.5
        self._fixed_orientation = False
        self._terminate = True

        self._client = actionlib.SimpleActionClient(self._topic,
                                                    DoAdaptionAction)
        Logger.loginfo('Waiting for adaption server ...')
        self._client.wait_for_server()
        Logger.loginfo('found adaption server!')

        self._error = False
示例#52
0
    def execute(self, userdata):
        '''
		Execute this state
		'''
        if self._failed:
            return 'failed'
        if self._reached:
            return 'reached'

        if self._client.has_result(self._action_topic):
            result = self._client.get_result(self._action_topic)
            if result.result == 1:
                self._reached = True
                return 'reached'
            else:
                self._failed = True
                Logger.logwarn('Failed to reach waypoint!')
                return 'failed'
	def calc_rotation_angle(self, affordance):
		'''Calculates the angle for the next planning step'''
		if ( self._state_machine.userdata.move_to_poses ):
			new_rotation_angle = math.radians(self.angle_increment_deg)
			self.total_displacement += new_rotation_angle
		else:
			new_rotation_angle = affordance.displacement + math.radians(self.angle_increment_deg)
			self.total_displacement = new_rotation_angle
		
		self._state_machine.userdata.current_target_angle_deg = self._state_machine.userdata.current_target_angle_deg + self.angle_increment_deg
		while ( self._state_machine.userdata.current_target_angle_deg >= 360 ):
			self._state_machine.userdata.current_target_angle_deg = self._state_machine.userdata.current_target_angle_deg - 360;
		
		affordance.displacement = new_rotation_angle
		
		Logger.loginfo("Next rotation step: %f degrees (%f increment from current position)" % (self._state_machine.userdata.current_target_angle_deg, math.degrees(new_rotation_angle)))
		
		return affordance
示例#54
0
    def on_exit(self, userdata):
        self.cmd_pub.linear.x = 0.0
        self.pub.publish(self.vel_topic, self.cmd_pub)

        self.cmd_status = RobotStatus()
        self.cmd_status.currentTask = ''
        self.cmd_status.currentBehaviorStatus = ''
        self.cmd_status.lastTask = ''
        self.cmd_status.lastTaskResult = ''
        self.cmd_status.nextTask = ''
        self.cmd_status.allActions = []
        self.cmd_status.indexCurrent = -1
        self.cmd_status.actionResults = []
        self.cmd_status.batteryCapacity = 0.0
        self.cmd_status.batteryVoltage = 0.0
        self.cmd_status.listen = False

        Logger.loginfo("Drive FWD ENDED!")
 def execute(self, userdata):
     '''
     Execute this state
     '''
     if self._param_error:
         return 'param_error'
     if self._request_failed:
         return 'request_failed'
     if self._response.error_code.val != MoveItErrorCodes.SUCCESS:
         Logger.logwarn('Move action failed with result error code: %s' %
                        str(self._response.error_code))
         self._moveit_failed = True
         return 'moveit_failed'
     else:
         Logger.loginfo('Move action succeeded: %s' %
                        str(self._response.error_code))
         self._success = True
         return 'reached'
示例#56
0
    def setGoal(self, pose):

        rospy.wait_for_service('/move_base/clear_costmaps')
        serv = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
        serv()
        goal = MoveBaseGoal()

        goal.target_pose.pose = pose

        goal.target_pose.header.frame_id = "base_link"
        # goal.target_pose.header.stamp.secs = 5.0

        # Send the action goal for execution
        try:
            self._client.send_goal(self._action_topic, goal)
        except Exception as e:
            Logger.logwarn("Unable to send navigation action goal:\n%s" % str(e))
            self._failed = True
示例#57
0
	def on_enter(self, userdata):
		# When entering this state, we send the action goal once to let the robot start its work.

		# As documented above, we get the specification of which dishwasher to use as input key.
		# This enables a previous state to make this decision during runtime and provide the ID as its own output key.
		
		# Create the goal.
		

		# Send the goal.
		self._error = False # make sure to reset the error state since a previous state execution might have failed
		try:
			self._client.send_goal(self._topic, goal)
		except Exception as e:
			# Since a state failure not necessarily causes a behavior failure, it is recommended to only print warnings, not errors.
			# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
			Logger.logwarn('Failed to send the move command:\n%s' % str(e))
			self._error = True
示例#58
0
    def execute(self, userdata):
        if self._active:
            """Wait for action result and return outcome accordingly"""

            self.Entity = None

            # Get the entity's object in the latest detections
            if self._sub.has_msg(self.entities_topic):
                message = self._sub.get_last_msg(self.entities_topic)
                for entity in message.entities:
                    if entity.ID == userdata.ID:
                        self.Entity = entity
                        print("entity found: "+str(entity.ID))

            # If entity is defined, get the direction to it
            if self.Entity:

                if (self.Entity.name == "person"):
                    if (self.Entity.face.id != ""):
                        self.Entity.position.z += self.Entity.face.boundingBox.Center.z-0.1
                    else:
                        self.Entity.position.z += 1.6

                ms = Float64()
                serv = rospy.ServiceProxy('/get_direction', get_direction)
                self.service.point = self.Entity.position
                resp = serv(self.service)
                # Publish to both Yaw and Pitch controllers
                ms.data = min(max(-resp.pitch, -0), 1)
                self.pubp.publish(ms)
                ms.data = min(max(resp.yaw, -1.2), 1.2)
                self.puby.publish(ms)
                return
        else:
            Logger.logwarn("simulation mode: let's assume I'm looking at something")
            return

        ms = Float64()
        ms.data = 0.3
        self.pubp.publish(ms)
        ms.data = 0
        self.puby.publish(ms)

        return "failed"
示例#59
0
    def on_start(self):

        self._return = None  # Clear completion flag

        # Wait for odometry message
        while (not self._odom_sub.has_msg(self._odom_topic)):
            Logger.logwarn('Waiting for odometry message from the robot ')
            rospy.sleep(0.05)

        while (not self._odom_sub.has_msg(self._odom_topic)):
            Logger.logwarn(
                'Waiting for odometry message to become available from the robot '
            )
            rospy.sleep(0.25)

        self._last_odom = self._sub.get_last_msg(self._odom_topic)
        self._odom_frame = self._last_odom.header.frame_id
        #Logger.loginfo('   odometry frame id <%s>' % (self._odom_frame))

        # Update the target transformation
        self._target.header.stamp = self._last_odom.header.stamp
        while (self.transformOdom(self._target) is None):
            Logger.logwarn(
                'Waiting for tf transformations to odometry frame to become available from the robot '
            )
            rospy.sleep(0.25)
            self._target.header.stamp = rospy.Time.now()

        while (self.transformMap(self._last_odom) is None):
            Logger.logwarn(
                'Waiting for tf transformations to map frame become available from the robot '
            )
            rospy.sleep(0.25)
            self._last_odom = self._sub.get_last_msg(self._odom_topic)

        self._current_position = self.transformMap(self._last_odom)

        # This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
        if (self._marker_pub):
            self._marker.action = Marker.ADD
            self._marker.color.a = 1.0  # Set alpha otherwise the marker is invisible
            self._marker.color.r = 0.0
            self._marker.color.g = 0.0
            self._marker.color.b = 1.0  # Indicate this target is planned
            self._marker_pub.publish(self._marker_topic, self._marker)
            self._marker_pub.publish(self._marker_topic,
                                     self._reference_marker)
            self._marker_pub.publish(self._marker_topic,
                                     self._local_target_marker)
            self._marker.action = Marker.MODIFY
            self._reference_marker.action = Marker.MODIFY
            self._reference_marker.color.a = 1.0  # Set alpha so it will become visible on next publish
            self._local_target_marker.action = Marker.MODIFY
            self._local_target_marker.color.a = 1.0  # Set alpha so it will become visible on next publish
    def __init__(self,
                 timeout=5.0,
                 enter_wait_duration=0.0,
                 action_topic=None):
        '''
        Constructor
        '''
        super(JointValuesToMoveItPlanState,
              self).__init__(outcomes=[
                  'planned', 'failed', 'topics_unavailable', 'param_error'
              ],
                             input_keys=[
                                 'action_topic', 'move_group', 'joint_names',
                                 'joint_values'
                             ],
                             output_keys=['joint_trajectory'])

        self.timeout_duration = rospy.Duration(timeout)
        self.enter_wait_duration = enter_wait_duration
        self.given_action_topic = action_topic
        self.moveit_client = None
        self.action_client = None
        self.return_code = None
        self.move_group = None
        self.joint_names = None
        self.joint_values = None

        try:
            self.moveit_client = ProxyMoveItClient(None)

            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.moveit_client.setup_action_client(
                    self.given_action_topic, "MoveGroupAction",
                    self.enter_wait_duration)
            else:
                self.given_action_topic = None  # override empty string

        except Exception as e:
            Logger.logerr(" Exception on initialization of %s\n %s" %
                          (self.name, str(e)))

        self.current_action_topic = self.given_action_topic