def __init__(self, reconfig_server, rate=100.0): self._dyn = reconfig_server self.continuous = self._dyn.config['continuous'] self._fjt = '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._fjt, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() # Current joint states self._positions = {} self._velocities = {} # Actual robot control self.states = EdoStates() rospy.Subscriber("joint_states", JointState, self._js_callback) # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments, messages, and dynamic reconfigure self._control_rate = rate # Hz self._update_rate_spinner = rospy.Rate(self._control_rate) self._control_joints = [] self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() # Start the action server rospy.sleep(0.5) while True: if self.states.edo_current_state in [ self.states.CS_CALIBRATED, self.states.CS_BRAKED ]: self._server.start() self._alive = True rospy.loginfo("Trayectory action server started") break else: rospy.logerr( "Joint Trajectory Action Server cannot be started when robot is in state %s" % self.states.get_current_code_string()) rospy.logerr( "Make sure your robot is started and calibrated properly with calibrate.launch" ) rospy.logerr("Trying again in 1 second... ") rospy.sleep(1)
def __init__(self): self._name_space = 'grip/gripper_action' # Start Action Server. self._server = actionlib.SimpleActionServer( self._name_space, GripperCommandAction, execute_cb=self._on_gripper_action, auto_start=False) self._action_name = rospy.get_name() self._server.start() # initialize a edo states in the action server self.states = EdoStates() # Action Feedback/Result self._feedback = GripperCommandFeedback() self._result = GripperCommandResult() self._timeout = 5.0 self._action_name = rospy.get_name() self._update_rate_spinner = rospy.Rate(20)
#!/usr/bin/env python import rospy import traceback from edo.states import EdoStates if __name__ == '__main__': rospy.init_node('edo_calibrate', anonymous=True) states = EdoStates(enable_algorithm_node=True) calibrated = False rospy.logwarn("Starting robot calibration procedure...") try: calibrated = states.calibration() except: traceback.print_exc() rospy.logerr("Robot calibration did not finish") else: if calibrated: rospy.logwarn("Robot is calibrated!") else: rospy.logerr("Robot calibration did not finish successfully")