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