コード例 #1
0
ファイル: action_nav.py プロジェクト: JMU-CS354/random_nav
    def goto_point(self, x_target, y_target, theta_target=0):
        """ Move to a location relative to the robot's current position """

        rospy.loginfo("navigating to: ({},{})".format(x_target, y_target))

        goal = create_goal_message(x_target, y_target, theta_target,
                                   'base_link')

        rospy.loginfo("Waiting for server.")
        self.ac.wait_for_server()

        rospy.loginfo("Sending goal.")
        self.ac.send_goal(goal)
        rospy.loginfo("Goal Sent.")

        # Check in after a while to see how things are going.
        rospy.sleep(1.0)
        rospy.loginfo("Status Text: {}".format(self.ac.get_goal_status_text()))

        # Should be either "ACTIVE", "SUCCEEDED" or "ABORTED"
        state_name = actionlib.get_name_of_constant(GoalStatus,
                                                    self.ac.get_state())
        rospy.loginfo("State      : {}".format(state_name))

        # Wait until the server reports a result.
        self.ac.wait_for_result()
        rospy.loginfo("Status Text: {}".format(self.ac.get_goal_status_text()))

        # Should be either "SUCCEEDED" or "ABORTED"
        state_name = actionlib.get_name_of_constant(GoalStatus,
                                                    self.ac.get_state())
        rospy.loginfo("State      : {}".format(state_name))
コード例 #2
0
    def _on_done(self, terminal_state, result):
        from actionlib import TerminalState, get_name_of_constant
        from copy import deepcopy
        from control_msgs.msg import FollowJointTrajectoryResult

        exception = TrajectoryExecutionFailed(
            'Trajectory execution failed ({:s}): {:s}'.format(
                get_name_of_constant(TerminalState, terminal_state),
                get_name_of_constant(FollowJointTrajectoryResult,
                                     result.error_code)
            ),
            executed=self.partial_result(),
            requested=deepcopy(self._traj_requested)

        )

        if terminal_state == TerminalState.SUCCEEDED:
            # Trajectory execution succeeded. Return the trajectory.
            if result.error_code == FollowJointTrajectoryResult.SUCCESSFUL:
                self.set_result(self._traj_executed)
            # Trajectory execution failed. Raise an exception.
            else:
                self.set_exception(exception)
        # Goal was cancelled. Note that this could have been one by another
        # thread or process, so _cancelled may be False.
        elif terminal_state in [TerminalState.PREEMPTED, TerminalState.RECALLED]:
            self.set_cancelled()
        else:
            self.set_exception(exception)
コード例 #3
0
    def _on_done(self, terminal_state, result):
        from actionlib import TerminalState, get_name_of_constant
        from pr_control_msgs.msg import TriggerActionResult

        exception = TriggerFailed(
            'Trigger action failed ({:s}): {:s}'.format(
                get_name_of_constant(TerminalState, terminal_state),
                get_name_of_constant(TriggerActionResult, result.success)))

        if terminal_state == TerminalState.SUCCEEDED:
            if result.success is True:
                self.set_result(result)
            else:
                self.set_exception(exception)
        elif terminal_state in [TerminalState.REJECTED,
                                TerminalState.RECALLED,
                                TerminalState.PREEMPTED]:
            self.set_cancelled()
        else:
            self.set_exception(exception)
コード例 #4
0
    def _on_done(self, terminal_state, result):
        from actionlib import TerminalState, get_name_of_constant
        from pr_control_msgs.msg import SetPositionActionResult

        exception = SetPositionFailed(
            'SetPosition action failed ({:s}): {:s}'.format(
                get_name_of_constant(TerminalState, terminal_state),
                get_name_of_constant(SetPositionActionResult, result.success)),
            executed=self._position_cmd,
            requested=self._position_cmd)

        if terminal_state == TerminalState.SUCCEEDED:
            if result.success is True:
                self.set_result(result)
            else:
                self.set_exception(exception)
        elif terminal_state in [TerminalState.REJECTED,
                                TerminalState.RECALLED,
                                TerminalState.PREEMPTED]:
            self.set_cancelled()
        else:
            self.set_exception(exception)
コード例 #5
0
 def done_cb(self, status, result):
     if status == GoalStatus.SUCCEEDED:
         rospy.loginfo("Replication suceeded")
         self.insert_replicate_date()
     else:
         rospy.logwarn("Replication finished with status %s" % actionlib.get_name_of_constant(GoalStatus, status))
コード例 #6
0
    arm_joint_action_client.wait_for_server()

    arm_goal = JointTrajectoryGoal()
    arm_goal.trajectory.joint_names = [
        'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
        'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint',
        'r_wrist_flex_joint', 'r_wrist_roll_joint'
    ]

    arm_pt = JointTrajectoryPoint()
    arm_pt.positions = [-pi / 2, 0, 0, 0, 0, 0, 0]
    arm_pt.time_from_start = rospy.Duration(5.0)
    arm_goal.trajectory.points.append(arm_pt)

    arm_joint_action_client.send_goal_and_wait(arm_goal)
    print 'arm action state:', actionlib.get_name_of_constant(
        actionlib.GoalStatus, arm_joint_action_client.get_state())

    print 'moving the left arm to problematic configuration'
    whicharm = 'l'
    arm_joint_trajectory_action_name = '' + whicharm + '_arm_controller/joint_trajectory_action'
    arm_joint_action_client = actionlib.SimpleActionClient(
        arm_joint_trajectory_action_name, JointTrajectoryAction)
    print 'waiting for', arm_joint_trajectory_action_name
    print 'have you set ROS_IP?'
    arm_joint_action_client.wait_for_server()
    torso_joint_trajectory_action_name = 'torso_controller/joint_trajectory_action'
    torso_joint_action_client = actionlib.SimpleActionClient(
        torso_joint_trajectory_action_name, JointTrajectoryAction)
    print 'waiting for', torso_joint_trajectory_action_name
    torso_joint_action_client.wait_for_server()
    print 'done init ROS'