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