class ReadyArmActionServer:
def __init__(self):
self.move_arm_client = SimpleActionClient('/move_left_arm',
MoveArmAction)
self.ready_arm_server = SimpleActionServer(ACTION_NAME,
ReadyArmAction,
execute_cb=self.ready_arm,
auto_start=False)
def initialize(self):
rospy.loginfo('%s: waiting for move_left_arm action server',
ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server',
ACTION_NAME)
self.ready_arm_server.start()
def ready_arm(self, goal):
if self.ready_arm_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.move_arm_client.cancel_goal()
self.ready_arm_server.set_preempted()
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
READY_POSITION,
collision_operations=goal.collision_operations):
self.ready_arm_server.set_succeeded()
else:
rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME)
self.ready_arm_server.set_aborted()
class ReadyArmActionServer:
def __init__(self):
self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction)
self.ready_arm_server = SimpleActionServer(ACTION_NAME,
ReadyArmAction,
execute_cb=self.ready_arm,
auto_start=False)
def initialize(self):
rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME)
self.ready_arm_server.start()
def ready_arm(self, goal):
if self.ready_arm_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.move_arm_client.cancel_goal()
self.ready_arm_server.set_preempted()
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
READY_POSITION,
collision_operations=goal.collision_operations):
self.ready_arm_server.set_succeeded()
else:
rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME)
self.ready_arm_server.set_aborted()
class axGripperServer:
def __init__(self, name):
self.fullname = name
self.goalAngle = 0.0
self.actualAngle = 0.0
self.failureState = False
dynamixelChain.move_angles_sync(ids[6:], [self.goalAngle], [0.5])
self.server = SimpleActionServer(
self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False
)
self.server.start()
def execute_cb(self, goal):
rospy.loginfo(goal)
self.currentAngle = goal.command.position
dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5])
attempts = 0
for i in range(10):
rospy.sleep(0.1)
attempts += 1
# while ... todo: add some condition to check on the actual angle
# rospy.sleep(0.1)
# print jPositions[4]
# attempts += 1
if attempts < 20:
self.server.set_succeeded()
else:
self.server.set_aborted()
def checkFailureState(self):
if self.failureState:
print "I am currently in a failure state."
class BaseRotate():
def __init__(self):
self._action_name = 'base_rotate'
#initialize base controller
topic_name = '/base_controller/command'
self._base_publisher = rospy.Publisher(topic_name, Twist)
#initialize this client
self._as = SimpleActionServer(self._action_name, BaseRotateAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def run(self, goal):
rospy.loginfo('Rotating base')
count = 0
r = rospy.Rate(10)
while count < 70:
if self._as.is_preempt_requested():
self._as.set_preempted()
return
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 1.0)
self._base_publisher.publish(twist_msg)
count = count + 1
r.sleep()
self._as.set_succeeded()
class PipolFollower():
def __init__(self):
rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS + "'")
self._as = SimpleActionServer(PIPOL_FOLLOWER_AS, PipolFollowAction,
execute_cb = self.execute_cb,
preempt_callback = self.preempt_cb,
auto_start = False)
rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS)
self._as.start()
def execute_cb(self, goal):
print "goal is: " + str(goal)
# helper variables
success = True
# start executing the action
for i in xrange(1, goal.order):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
class OmniPoseFollower(object):
def __init__(self):
self.server = SimpleActionServer(
'/whole_body_controller/refills_finger/follow_joint_trajectory',
FollowJointTrajectoryAction,
self.execute_cb,
auto_start=False)
self.state_pub = rospy.Publisher('refills_finger/state',
JointTrajectoryControllerState,
queue_size=10)
self.js_sub = rospy.Subscriber('refills_finger/joint_states',
JointState,
self.js_cb,
queue_size=10)
self.server.start()
def js_cb(self, data):
msg = JointTrajectoryControllerState()
msg.joint_names = data.name
self.state_pub.publish(msg)
def execute_cb(self, data):
"""
:type data: FollowJointTrajectoryGoal
:return:
"""
self.server.set_succeeded()
class TrajectoryExecution:
def __init__(self, side_prefix):
traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints)
self.action_server.start()
self.has_goal = False
def move_to_joints(self, traj_goal):
rospy.loginfo('Receieved a trajectory execution request.')
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
self.traj_action_client.send_goal(traj_goal)
rospy.sleep(1)
is_terminated = False
while(not is_terminated):
action_state = self.traj_action_client.get_state()
if (action_state == GoalStatus.SUCCEEDED):
self.action_server.set_succeeded()
is_terminated = True
elif (action_state == GoalStatus.ABORTED):
self.action_server.set_aborted()
is_terminated = True
elif (action_state == GoalStatus.PREEMPTED):
self.action_server.set_preempted()
is_terminated = True
rospy.loginfo('Trajectory completed.')
class axGripperServer:
def __init__(self, name):
self.fullname = name
self.currentAngle = 0.0
# dynamixelChain.move_angle(7, 0.0, 0.5)
dynamixelChain.move_angles_sync(ids[6:], [0.0], [0.5])
self.server = SimpleActionServer(self.fullname,
GripperCommandAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def execute_cb(self, goal):
rospy.loginfo(goal)
self.currentAngle = goal.command.position
dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5])
# dynamixelChain.move_angle(7, 0.1, 0.5)
rospy.sleep(0.1)
print jPositions[4]
attempts = 0
while abs(goal.command.position - jPositions[4]) > 0.1 and attempts < 20:
rospy.sleep(0.1)
print jPositions[4]
attempts += 1
if attempts < 20:
self.server.set_succeeded()
else:
self.server.set_aborted()
class MockActionServer(object):
""" MockActionServer base class """
def __init__(self, name, topic, action_type):
""" Creating a custom mock action server."""
self._topic = topic
self._name = name
self._action_type = action_type
self.timeout = 5
self.action_result = None
Subscriber('mock/' + name, String, self.receive_commands)
Subscriber('mock/gui_result', Bool, self.set_gui_result)
self._server = ActionServer(self._topic, self._action_type,
self.success, False)
self._server.start()
loginfo('>>> Starting ' + self._name)
def receive_commands(self, msg):
""" Decides the result of the next call. """
callback, timeout = msg.data.split(':')
self.timeout = float(timeout)
self._server.execute_callback = getattr(self, callback)
logwarn('>>> ' + self._name + ': Current callback -> ' + callback)
sleep(1)
def abort(self, goal):
""" Aborts any incoming goal. """
logwarn('>>> ' + self._name + ': This goal will be aborted.')
sleep(self.timeout)
self._server.set_aborted()
def success(self, goal):
""" Succeeds any incoming goal. """
logwarn('>>> ' + self._name + ': This goal will succeed.')
sleep(self.timeout)
self._server.set_succeeded(self.action_result)
def preempt(self, goal):
""" Preempts any incoming goal. """
logwarn('>>> ' + self._name + ': This goal will be preempted.')
sleep(self.timeout)
self._server.set_preempted()
def set_gui_result(self, msg):
""" Sets the result of the goal. """
self.action_result = ValidateVictimGUIResult()
logwarn('>>> The gui response will be: ' + str(msg.data))
self.action_result.victimValid = msg.data
class SimulatedVisionServer(object):
def __init__(self,
object_names,
frame_id="world",
action_ns="recognize_objects"):
self.server = SimpleActionServer(action_ns,
ObjectRecognitionAction,
execute_cb=self.execute_callback)
self.recognized_objects = dict()
self.object_names = object_names
self.listener = PayloadTransformListener()
self.frame_id = "world"
def execute_callback(self, goal):
now = rospy.Time.now()
r_array = RecognizedObjectArray()
r_array.header.stamp = now
r_array.header.frame_id = self.frame_id
if goal.use_roi:
raise Warning("use_roi in ObjectRecognitionRequest ignored")
for o in self.object_names:
try:
object_tf = self.listener.lookupTransform(
"/world", o, rospy.Time(0))
print o
print object_tf
print ""
p = PoseWithCovarianceStamped()
p.pose.pose = rox_msg.transform2pose_msg(object_tf)
p.header.stamp = now
p.header.frame_id = self.frame_id
r = RecognizedObject()
r.header.stamp = now
r.header.frame_id = self.frame_id
r.type.key = o
r.confidence = 1
r.pose = p
r_array.objects.append(r)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
result = ObjectRecognitionResult()
result.recognized_objects = r_array
self.server.set_succeeded(result=result)
class LookAndMoveNode(object):
cmd_vel = Twist()
def __init__(self):
self.tf_listener = tf.TransformListener()
self.pub_cmd_vel = rospy.Publisher('cmd_vel',Twist,queue_size=1)
self.sub_odom = rospy.Subscriber('odom',Odometry,callback=self.OdomCB)
self.is_stop = False
self.target_pose = PoseStamped()
self.observe_frame = ''
self.state = LookAndMoveStatus.IDLE
self._as = SimpleActionServer("look_n_move_node_action", LookAndMoveAction, self.ExecuteCB, auto_start=False)
self._as.start()
def ExecuteCB(self,goal):
print 'LOOKnMOVE GOAL RCV'
ps = goal.relative_pose
self.observe_frame = ps.header.frame_id if len(ps.header.frame_id)!=0 else 'base_link'
ps.header.frame_id = self.observe_frame
ps.header.stamp = rospy.Time(0)
self.tf_listener.waitForTransform(self.observe_frame, 'odom', rospy.Time(0),rospy.Duration(1.0))
self.target_pose = self.tf_listener.transformPose('odom',ps)
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
print 'PREEMPT REQ'
rospy.sleep(0.1)
self.SendCmdVel(0.,0.,0.)
self._as.set_preempted()
return
self.tf_listener.waitForTransform('odom','base_link',rospy.Time(0),rospy.Duration(1.0))
pose_base = self.tf_listener.transformPose('base_link',self.target_pose)
pose = pose_base.pose.position
q = pose_base.pose.orientation
yaw = tf.transformations.euler_from_quaternion([q.x,q.y,q.z,q.w])[2]
if (np.abs(pose.x) < 0.02 and np.abs(pose.y) <0.02 and np.abs(yaw)<0.01) or ((np.abs(pose.x) < 0.05 and np.abs(pose.y) <0.05 and np.abs(yaw)<0.04) and self.is_stop):
print 'SUCCESS'
rospy.sleep(0.1)
self.SendCmdVel(0.,0.,0.)
self._as.set_succeeded()
return
vx = pose.x * KP_X
vy = pose.y * KP_Y
yaw = yaw * KP_YAW
self.SendCmdVel(vx,vy,yaw)
def OdomCB(self,data):
self.is_stop = data.twist.twist.linear.x < 0.001 and data.twist.twist.linear.y < 0.001
def SendCmdVel(self, vx, vy, vyaw):
self.cmd_vel.linear.x = np.clip(vx, -MAX_LINEAR_VEL, MAX_LINEAR_VEL)
self.cmd_vel.linear.y = np.clip(vy, -MAX_LINEAR_VEL, MAX_LINEAR_VEL)
self.cmd_vel.angular.z = np.clip(vyaw, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL)
self.pub_cmd_vel.publish(self.cmd_vel)
class TestServer:
def __init__(self,name):
self._sas = SimpleActionServer(name,
TestAction,
execute_cb=self.execute_cb)
def execute_cb(self, msg):
if msg.goal == 0:
self._sas.set_succeeded()
elif msg.goal == 1:
self._sas.set_aborted()
class MockActionServer(object):
""" MockActionServer base class """
def __init__(self, name, topic, action_type):
""" Creating a custom mock action server."""
self._topic = topic
self._name = name
self._action_type = action_type
self.timeout = 1
self.action_result = None
self.prefered_callback = ' '
Subscriber('mock/' + name, String, self.receive_commands)
Subscriber('mock/gui_result', Bool, self.set_gui_result)
self._server = ActionServer(self._topic, self._action_type,
self.decide, False)
self._server.start()
loginfo('>>> Starting ' + self._name)
def receive_commands(self, msg):
""" Decides the result of the next call. """
callback, timeout = msg.data.split(':')
# self.timeout = float(timeout)
self.prefered_callback = callback
sleep(1)
def decide(self, goal):
""" Deciding outcome of goal """
logwarn('Deciding callback...')
sleep(self.timeout)
if(self.prefered_callback == 'success'):
logwarn('>>> ' + self._name + ': This goal will succeed.')
self._server.set_succeeded()
elif(self.prefered_callback == 'abort'):
logwarn('>>> ' + self._name + ': This goal will be aborted.')
self._server.set_aborted()
elif(self.prefered_callback == 'preempt'):
logwarn('>>> ' + self._name + ': This goal will be preempted.')
self._server.set_preempted()
else:
logwarn('wtf?')
def set_gui_result(self, msg):
""" Sets the result of the goal. """
self.action_result = ValidateVictimGUIResult()
logwarn('>>> The gui response will be: ' + str(msg.data))
self.action_result.victimValid = msg.data
class ScanTableActionServer:
"""
Scan Table Mock
Run this file to have mock to the action '/head_traj_controller/head_scan_snapshot_action '(Scan Table)
"""
def __init__(self):
a_scan_table = {'name': '/head_traj_controller/head_scan_snapshot_action', 'ActionSpec': PointHeadAction, 'execute_cb': self.scan_table_cb, 'auto_start': False}
self.s = SimpleActionServer(**a_scan_table)
self.s.start()
def scan_table_cb(self, req):
rospy.loginfo('Scan Table \'/head_traj_controller/head_scan_snapshot_action was called.')
self.s.set_succeeded() if bool(random.randint(0, 1)) else self.s.set_aborted()
class ybGripperServer:
def __init__(self, name):
self.fullname = name
self.currentValue = 0.0
gripperTopic = '/arm_1/gripper_controller/position_command'
self.jppub = rospy.Publisher(gripperTopic, JointPositions)
self.server = SimpleActionServer(self.fullname,
GripperCommandAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def execute_cb(self, goal):
rospy.loginfo(goal)
self.currentValue = goal.command.position
self.moveGripper(self.jppub, self.currentValue)
attempts = 0
# here we should be checking if the gripper has gotten to its goal
for i in range(5):
rospy.sleep(0.1)
attempts += 1
if attempts < 20:
self.server.set_succeeded()
else:
self.server.set_aborted()
def moveGripper(self, gPublisher, floatVal):
jp = JointPositions()
myPoison = Poison()
myPoison.originator = 'yb_grip'
myPoison.description = 'whoknows'
myPoison.qos = 0.0
jp.poisonStamp = myPoison
nowTime = rospy.Time.now()
jvl = JointValue()
jvl.timeStamp = nowTime
jvl.joint_uri = 'gripper_finger_joint_l'
jvl.unit = 'm'
jvl.value = floatVal
jp.positions.append(jvl)
jvr = JointValue()
jvr.timeStamp = nowTime
jvr.joint_uri = 'gripper_finger_joint_r'
jvr.unit = 'm'
jvr.value = floatVal
jp.positions.append(jvr)
gPublisher.publish(jp)
def ybspin(self):
rospy.sleep(0.1)
class AveragingSVR2(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.register_goal_callback(self.goal_cb)
self.reset_numbers()
rospy.Subscriber('number', Float32, self.execute_loop)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def goal_cb(self):
self._goal = self._action.accept_new_goal()
rospy.loginfo('goal callback %s' % (self._goal))
def preempt_cb(self):
rospy.loginfo('preempt callback')
self.reset_numbers()
self._action.set_preempted(text='message for preempt')
def reset_numbers(self):
self._samples = []
def execute_loop(self, msg):
if (not self._action.is_active()):
return
self._samples.append(msg.data)
feedback = AveragingAction().action_feedback.feedback
feedback.sample = len(self._samples)
feedback.data = msg.data
feedback.mean = sum(self._samples) / len(self._samples)
feedback.std_dev = self.std_dev(self._samples)
self._action.publish_feedback(feedback)
## sending result
if (len(self._samples) >= self._goal.samples):
result = AveragingAction().action_result.result
result.mean = sum(self._samples) / len(self._samples)
result.std_dev = self.std_dev(self._samples)
rospy.loginfo('result: %s' % (result))
self.reset_numbers()
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result,
text='message for aborted')
class FibonacciActionServer(object):
# create messages that are used to publish feedback/result
feedback = FibonacciFeedback()
result = FibonacciResult()
def __init__(self, name):
self.action_name = name
self.action_server = SimpleActionServer(self.action_name,
FibonacciAction,
execute_cb=self.execute_cb,
auto_start=False)
self.action_server.start()
def execute_cb(self, goal):
# helper variables
r = rospy.Rate(1)
success = True
# append the seeds for the fibonacci sequence
self.feedback.sequence = []
self.feedback.sequence.append(0)
self.feedback.sequence.append(1)
# publish info to the console for the user
rospy.loginfo(
'%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i'
% (self.action_name, goal.order, self.feedback.sequence[0],
self.feedback.sequence[1]))
# start executing the action
for i in range(1, goal.order):
# check that preempt has not been requested by the client
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self.action_name)
self.action_server.set_preempted()
success = False
break
self.feedback.sequence.append(self.feedback.sequence[i] +
self.feedback.sequence[i - 1])
# publish the feedback
rospy.loginfo('publishing feedback ...')
self.action_server.publish_feedback(self.feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self.result.sequence = self.feedback.sequence
rospy.loginfo('%s: Succeeded' % self.action_name)
self.action_server.set_succeeded(self.result)
class GripperController:
def __init__(self, hand):
arm = hand[0]
self.effort = -1.0
self.server = SimpleActionServer(ACTION_NAME % arm,
GripperSequenceAction, self.execute,
False)
self.server.start()
self.sub_client = SimpleActionClient(
"%s_gripper_controller/gripper_action" % arm,
Pr2GripperCommandAction)
#wait for the action servers to come up
rospy.loginfo("[GRIPPER] Waiting for %s controllers" % arm)
self.sub_client.wait_for_server()
rospy.loginfo("[GRIPPER] Got %s controllers" % arm)
self.client = SimpleActionClient(ACTION_NAME % arm,
GripperSequenceAction)
rospy.loginfo("[GRIPPER] Waiting for self client")
self.client.wait_for_server()
rospy.loginfo("[GRIPPER] Got self client")
def send_goal(self, goal):
self.client.send_goal(goal)
def execute(self, goal):
rate = rospy.Rate(10)
while rospy.Time.now() < goal.header.stamp:
rate.sleep()
for position, time in zip(goal.positions, goal.times):
self.change_position(position, False)
rospy.sleep(time)
self.server.set_succeeded(GripperSequenceResult())
self.change_position(position, False, 0.0)
def change_position(self, position, should_wait=True, effort=None):
if effort is None:
effort = self.effort
gg = Pr2GripperCommandGoal()
gg.command.position = position
gg.command.max_effort = effort
self.sub_client.send_goal(gg)
if should_wait:
self.sub_client.wait_for_result()
class SynchronizedSimpleActionServer(object):
def __init__(self, namespace, action_spec, execute_cb):
self.goal_service = rospy.Service(namespace + '/get_goal_from_id',
GetTaskFromID, self.task_id_cb)
self.server = SimpleActionServer(namespace,
action_spec,
execute_cb,
auto_start=False)
self.server.start()
def task_id_cb(self, request):
idx = request.task_id
current_goal = self.server.current_goal
if idx == current_goal.goal_id.id:
return GetTaskFromIDResponse(current_goal.get_goal())
return GetTaskFromIDResponse()
def is_preempt_requested(self):
return self.server.is_preempt_requested()
def set_preempted(self):
return self.server.set_preempted()
def set_succeeded(self, result):
return self.server.set_succeeded(result)
def publish_feedback(self, feedback):
return self.server.publish_feedback(feedback)
class TestServerClass():
def __init__(self):
self.smach_action_server = SimpleActionServer('test_smach_action_server',testAction,
execute_cb=self.execute_cb,auto_start=False)
self.smach_action_server.start()
def execute_cb(self,goal):
sm = getStateMachine()
sm.userdata.action_goal = goal
smach_thread = Thread(target=sm.execute)
smach_thread.start()
while(smach_thread.is_alive()):
self.smach_action_server.publish_feedback(testFeedback(str(sm.get_active_states())))
rospy.loginfo(sm.get_active_states())
rospy.sleep(rospy.Duration(1.0))
self.smach_action_server.set_succeeded(testResult("The Task Got Completed"))
class MockGui():
def __init__(self, gui_validation_topic):
self.reply = False
self.preempted = 0
self.victimValid = True
self.victimFoundx = 0
self.victimFoundy = 0
self.probability = 0
self.sensorIDsFound = []
self.gui_validate_victim_as_ = SimpleActionServer(
gui_validation_topic,
ValidateVictimGUIAction,
execute_cb=self.gui_validate_victim_cb,
auto_start=False)
self.gui_validate_victim_as_.start()
def __del__(self):
self.gui_validate_victim_as_.__del__()
def gui_validate_victim_cb(self, goal):
rospy.loginfo('gui_validate_victim_cb')
self.reply = False
self.victimFoundx = goal.victimFoundx
self.victimFoundy = goal.victimFoundy
self.probability = goal.probability
self.sensorIDsFound = goal.sensorIDsFound
print goal
while not self.reply:
rospy.sleep(0.5)
if self.gui_validate_victim_as_.is_preempt_requested():
preempted += 1
self.gui_validate_victim_as_.set_preempted()
break
else:
self.preempted = 0
result = ValidateVictimGUIResult(victimValid=self.victimValid)
self.gui_validate_victim_as_.set_succeeded(result)
class ReemTabletopManipulationMock():
def __init__(self):
a_grasp_target_action = {'name': '/tabletop_grasping_node', 'ActionSpec': GraspTargetAction, 'execute_cb': self.grasp_target_action_cb, 'auto_start': False}
self.s = SimpleActionServer(**a_grasp_target_action)
self.s.start()
def grasp_target_action_cb(self, req):
rospy.loginfo('Grasp Target Action \'%s\' /tabletop_grasping_node was called.' % req.appearanceID)
res = GraspTargetResult()
res.detectionResult = TabletopDetectionResult()
res.detectionResult.models = [DatabaseModelPoseList()]
res.detectionResult.models[0].model_list = [DatabaseModelPose()]# = [0].model_id = req.databaseID
res.detectionResult.models[0].model_list[0].model_id = req.databaseID
self.s.set_succeeded(res) if bool(random.randint(0, 1)) else self.s.set_aborted(res)
class MockEndEffectorPlanner():
def __init__(self, end_effector_planner_topic):
self.moves_end_effector = False
self.move_end_effector_succeeded = False
self.reply = False
self.command = 0
self.point_of_interest = ""
self.center_point = ""
self.end_effector_planner_as_ = SimpleActionServer(
end_effector_planner_topic,
MoveEndEffectorAction,
execute_cb=self.end_effector_planner_cb,
auto_start=False)
self.end_effector_planner_as_.start()
def __del__(self):
self.end_effector_planner_as_.__del__()
def end_effector_planner_cb(self, goal):
rospy.loginfo('end_effector_planner_cb')
self.moves_end_effector = True
self.command = goal.command
self.point_of_interest = goal.point_of_interest
self.center_point = goal.center_point
while not self.reply:
rospy.sleep(1.)
if self.end_effector_planner_as_.is_preempt_requested():
self.end_effector_planner_as_.set_preempted(MoveEndEffectorResult())
self.moves_end_effector = False
return None
else:
self.reply = False
result = MoveEndEffectorResult()
if self.move_end_effector_succeeded:
self.moves_end_effector = False
self.end_effector_planner_as_.set_succeeded(result)
else:
self.moves_end_effector = False
self.end_effector_planner_as_.set_aborted(result)
class AveragingSVR(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
execute_cb=self.execute_cb,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def preempt_cb(self):
rospy.loginfo('preempt callback')
self._action.set_preempted(text='message for preempt')
def execute_cb(self, goal):
rospy.loginfo('execute callback: %s' % (goal))
feedback = AveragingAction().action_feedback.feedback
result = AveragingAction().action_result.result
## execute loop
rate = rospy.Rate(1 / (0.01 + 0.99 * random.random()))
samples = []
for i in range(goal.samples):
sample = random.random()
samples.append(sample)
feedback.sample = i
feedback.data = sample
feedback.mean = sum(samples) / len(samples)
feedback.std_dev = self.std_dev(samples)
self._action.publish_feedback(feedback)
rate.sleep()
if (not self._action.is_active()):
rospy.loginfo('not active')
return
## sending result
result.mean = sum(samples) / len(samples)
result.std_dev = self.std_dev(samples)
rospy.loginfo('result: %s' % (result))
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result, text='message for aborted')
class OpenDoor():
def __init__(self):
self.pub = rospy.Publisher('/cmd_vel_mux/input/teleop',
Twist,
queue_size=10)
self.sub = rospy.Subscriber('/scan', LaserScan, self.laserscanCB)
self.laser = LaserScan()
self.front_value = 999.9
self._action_server = SimpleActionServer('door_action',
OpenDoorAction,
execute_cb=self.execute,
auto_start=False)
self.flg = False
self.result = OpenDoorResult()
self._action_server.start()
def laserscanCB(self, receive_msg):
self.front_value = receive_msg.ranges[359]
self.flg = True
def linerContorol(self, value):
twist_cmd = Twist()
twist_cmd.linear.x = value
rospy.sleep(0.1)
self.pub.publish(twist_cmd)
def execute(self, goal):
try:
rospy.loginfo('start"door_action"')
while not rospy.is_shutdown() and self.flg == False:
rospy.loginfo('wait for laserscan ...')
rospy.sleep(2.0)
self.flg = False
while not rospy.is_shutdown() and self.front_value < 1.0:
rospy.sleep(1.0)
self.result.data = True
for i in range(10):
self.linerContorol(0.1)
self._action_server.set_succeeded(self.result)
except rospy.ROSInterruptException:
rospy.loginfo('Interrupted')
pass
class fibonacci_server:
def __init__(self, name):
self.name = name
self.feedback = FibonacciFeedback()
self.result = FibonacciResult()
self.action_server = SimpleActionServer(
self.name,
FibonacciAction,
execute_cb=self.execute_callback,
auto_start=False)
self.action_server.start()
def execute_callback(self, goal):
rate = rospy.Rate(1)
success = True
self.feedback.sequence[:] = []
self.feedback.sequence.append(0)
self.feedback.sequence.append(1)
rospy.loginfo(
'[{}] Executing, creating fibonacci sequence of order {} with seeds {}, {}'
.format(self.name, goal.order, self.feedback.sequence[0],
self.feedback.sequence[1]))
for i in range(1, goal.order):
if self.action_server.is_preempt_requested() or rospy.is_shutdown(
):
rospy.loginfo('[{}] Prempeted'.format(self.name))
success = False
self.action_server.set_preempted()
break
self.feedback.sequence.append(self.feedback.sequence[i - 1] +
self.feedback.sequence[i])
self.action_server.publish_feedback(self.feedback)
rate.sleep()
if success:
self.result.sequence = self.feedback.sequence
rospy.loginfo('[{}] Succeeded'.format(self.name))
self.action_server.set_succeeded(self.result)
class TrajectoryExecution:
def __init__(self, side_prefix):
traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server arm: ' +
side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.action_server = SimpleActionServer(motion_request_name,
JointTrajectoryAction,
execute_cb=self.move_to_joints)
self.action_server.start()
self.has_goal = False
def move_to_joints(self, traj_goal):
rospy.loginfo('Receieved a trajectory execution request.')
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
self.traj_action_client.send_goal(traj_goal)
rospy.sleep(1)
is_terminated = False
while (not is_terminated):
action_state = self.traj_action_client.get_state()
if (action_state == GoalStatus.SUCCEEDED):
self.action_server.set_succeeded()
is_terminated = True
elif (action_state == GoalStatus.ABORTED):
self.action_server.set_aborted()
is_terminated = True
elif (action_state == GoalStatus.PREEMPTED):
self.action_server.set_preempted()
is_terminated = True
rospy.loginfo('Trajectory completed.')
class OSMTopologicalPlannerNode(object):
def __init__(self):
server_ip = rospy.get_param('~overpass_server_ip')
server_port = rospy.get_param('~overpass_server_port')
ref_lat = rospy.get_param('~ref_latitude')
ref_lon = rospy.get_param('~ref_longitude')
building = rospy.get_param('~building')
global_origin = [ref_lat, ref_lon]
rospy.loginfo("Server " + server_ip + ":" + str(server_port))
rospy.loginfo("Global origin: " + str(global_origin))
rospy.loginfo("Starting servers...")
self.osm_topological_planner_server = SimpleActionServer(
'/osm_topological_planner', OSMTopologicalPlannerAction, self._osm_topological_planner, False)
self.osm_topological_planner_server.start()
osm_bridge = OSMBridge(
server_ip=server_ip,
server_port=server_port,
global_origin=global_origin,
coordinate_system="cartesian",
debug=False)
path_planner = PathPlanner(osm_bridge)
path_planner.set_building(building)
self.osm_topological_planner_callback = OSMTopologicalPlannerCallback(
osm_bridge, path_planner)
rospy.loginfo(
"OSM topological planner server started. Listening for requests...")
def _osm_topological_planner(self, req):
res = self.osm_topological_planner_callback.get_safe_response(req)
if res is not None:
self.osm_topological_planner_server.set_succeeded(res)
else:
self.osm_topological_planner_server.set_aborted(res)
class DataFusionServer(object):
def __init__(self, name, topic, action_type, result_type):
self.name = name
self.result = result_type()
self.topic = topic
self.action_type = action_type
self.timeout = 5
Subscriber('mock/' + name, String, self.receive_commands)
self.server = ActionServer(self.topic, self.action_type, self.success,
False)
self.server.start()
loginfo('>>> Starting ' + self.name)
def receive_commands(self, msg):
callback, timeout = msg.data.split(':')
self.timeout = float(timeout)
self.server.execute_callback = getattr(self, callback)
logwarn('>>> ' + self.name + ': Current callback -> ' + callback)
sleep(1)
def create_random_world_model(self):
victims = [create_victim_info(i + 1) for i in range(2)]
visited = [create_victim_info(i + 1) for i in range(4)]
self.result.worldModel = create_world_model(victims, visited)
def success(self, goal):
self.create_random_world_model()
logwarn('>>> ' + self.name + ': This goal will succeed.')
sleep(self.timeout)
self.server.set_succeeded(result=self.result)
def abort(self, goal):
self.create_random_world_model()
logwarn('>>> ' + self.name + ': This goal will be aborted.')
sleep(self.timeout)
self.server.set_aborted(result=self.result)
class PipolFollower():
def __init__(self):
rospy.loginfo("Creating Pipol follower AS: '" + PIPOL_FOLLOWER_AS +
"'")
self._as = SimpleActionServer(PIPOL_FOLLOWER_AS,
PipolFollowAction,
execute_cb=self.execute_cb,
preempt_callback=self.preempt_cb,
auto_start=False)
rospy.loginfo("Starting " + PIPOL_FOLLOWER_AS)
self._as.start()
def execute_cb(self, goal):
print "goal is: " + str(goal)
# helper variables
success = True
# start executing the action
for i in xrange(1, goal.order):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
self._feedback.sequence.append(self._feedback.sequence[i] +
self._feedback.sequence[i - 1])
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
class GraspObjectActionServer:
def __init__(self):
self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording)
self.grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning', GraspPlanning)
self.get_solver_info_srv = rospy.ServiceProxy('/wubble2_left_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
self.get_ik_srv = rospy.ServiceProxy('/wubble2_left_arm_kinematics/get_ik', GetPositionIK)
self.set_planning_scene_diff_client = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff)
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction)
self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject)
self.action_server = SimpleActionServer(ACTION_NAME,
GraspObjectAction,
execute_cb=self.grasp_object,
auto_start=False)
def initialize(self):
rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME)
rospy.wait_for_service('audio_dump/start_audio_recording')
rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME)
rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME)
rospy.wait_for_service('audio_dump/stop_audio_recording')
rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME)
rospy.loginfo('%s: waiting for GraspPlanning service' % ACTION_NAME)
rospy.wait_for_service('/GraspPlanning')
rospy.loginfo('%s: connected to GraspPlanning service' % ACTION_NAME)
rospy.loginfo('%s: waiting for wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME)
rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik_solver_info')
rospy.loginfo('%s: connected to wubble2_left_arm_kinematics/get_ik_solver_info service' % ACTION_NAME)
rospy.loginfo('%s: waiting for wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME)
rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik')
rospy.loginfo('%s: connected to wubble2_left_arm_kinematics/get_ik service' % ACTION_NAME)
rospy.loginfo('%s: waiting for environment_server/set_planning_scene_diff service' % ACTION_NAME)
rospy.wait_for_service('/environment_server/set_planning_scene_diff')
rospy.loginfo('%s: connected to set_planning_scene_diff service' % ACTION_NAME)
rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME)
self.posture_controller.wait_for_server()
rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME)
rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME)
rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME)
rospy.wait_for_service('/wubble_grasp_status')
rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME)
self.action_server.start()
def find_ik_for_grasping_pose(self, pose_stamped):
solver_info = self.get_solver_info_srv()
arm_joints = solver_info.kinematic_solver_info.joint_names
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = GRIPPER_LINK_FRAME
req.ik_request.pose_stamped = pose_stamped
try:
current_state = rospy.wait_for_message('/joint_states', JointState, 2.0)
req.ik_request.ik_seed_state.joint_state.name = arm_joints
req.ik_request.ik_seed_state.joint_state.position = [current_state.position[current_state.name.index(j)] for j in arm_joints]
if not self.set_planning_scene_diff_client():
rospy.logerr('%s: Find IK for Grasp: failed to set planning scene diff' % ACTION_NAME)
return None
ik_result = self.get_ik_srv(req)
if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS:
return ik_result.solution
else:
rospy.logerr('%s: failed to find an IK for requested grasping pose, aborting' % ACTION_NAME)
return None
except:
rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME)
return None
def find_grasp_pose(self, target, collision_object_name='', collision_support_surface_name=''):
"""
target = GraspableObject
collision_object_name = name of target in collision map
collision_support_surface_name = name of surface target is touching
"""
req = GraspPlanningRequest()
req.arm_name = ARM_GROUP_NAME
req.target = target
req.collision_object_name = collision_object_name
req.collision_support_surface_name = collision_support_surface_name
rospy.loginfo('%s: trying to find a good grasp for graspable object %s' % (ACTION_NAME, collision_object_name))
grasping_result = self.grasp_planning_srv(req)
if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS:
rospy.logerr('%s: unable to find a feasable grasp, aborting' % ACTION_NAME)
return None
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = grasping_result.grasps[0].grasp_posture.header.frame_id
pose_stamped.pose = grasping_result.grasps[0].grasp_pose
rospy.loginfo('%s: found good grasp, looking for corresponding IK' % ACTION_NAME)
return self.find_ik_for_grasping_pose(pose_stamped)
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
def close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
rospy.sleep(1)
grasp_status = self.get_grasp_status_srv()
return grasp_status.is_hand_occupied
def grasp_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
target = goal.graspable_object
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
ik_solution = self.find_grasp_pose(target, collision_object_name, collision_support_surface_name)
if ik_solution:
# disable collisions between gripper and target object
ordered_collision_operations = OrderedCollisionOperations()
collision_operation1 = CollisionOperation()
collision_operation1.object1 = collision_object_name
collision_operation1.object2 = GRIPPER_GROUP_NAME
collision_operation1.operation = CollisionOperation.DISABLE
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
# collision_operation2.penetration_distance = 0.02
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
# set gripper padding to 0
gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
gripper_paddings.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
self.open_gripper()
# move into pre-grasp pose
if not move_arm_joint_goal(self.move_arm_client,
ik_solution.joint_state.name,
ik_solution.joint_state.position,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
return
# record grasping sound with 0.5 second padding before and after
self.start_audio_recording_srv(InfomaxAction.GRASP, goal.category_id)
rospy.sleep(0.5)
grasp_successful = self.close_gripper()
rospy.sleep(0.5)
# if grasp was successful, attach it to the gripper
if grasp_successful:
sound_msg = self.stop_audio_recording_srv(True)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
self.action_server.set_succeeded(GraspObjectResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
class DropObjectActionServer:
def __init__(self):
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording)
self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject)
self.action_server = SimpleActionServer(ACTION_NAME,
DropObjectAction,
execute_cb=self.drop_object,
auto_start=False)
def initialize(self):
rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME)
rospy.wait_for_service('/wubble_grasp_status')
rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME)
rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME)
self.posture_controller.wait_for_server()
rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME)
rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME)
rospy.wait_for_service('audio_dump/start_audio_recording')
rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME)
rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME)
rospy.wait_for_service('audio_dump/stop_audio_recording')
rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME)
self.action_server.start()
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
def drop_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
# check that we have something in hand before dropping it
grasp_status = self.get_grasp_status_srv()
# if the object is still in hand after lift is done we are good
if not grasp_status.is_hand_occupied:
rospy.logerr('%s: gripper empty, nothing to drop' % ACTION_NAME)
self.action_server.set_aborted()
return
# record sound padded by 0.5 seconds from start and 1.5 seconds from the end
self.start_audio_recording_srv(InfomaxAction.DROP, goal.category_id)
rospy.sleep(0.5)
self.open_gripper()
rospy.sleep(1.5)
# check if gripper actually opened first
sound_msg = None
grasp_status = self.get_grasp_status_srv()
# if there something in the gripper - drop failed
if grasp_status.is_hand_occupied:
self.stop_audio_recording_srv(False)
else:
sound_msg = self.stop_audio_recording_srv(True)
# delete the object that we just dropped, we don't really know where it will land
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.REMOVE
obj.object.id = goal.collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
if sound_msg:
self.action_server.set_succeeded(DropObjectResult(sound_msg.recorded_sound))
else:
self.action_server.set_aborted()
class AStarSearch(object):
def __init__(self, name):
rospy.loginfo("Starting {}".format(name))
self._as = SimpleActionServer(name,
TopoNavAction,
self.execute_cb,
auto_start=False)
self._as.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient("/waypoint_nav_node",
WayPointNavAction)
rospy.loginfo("Waiting for nav server")
self.client.wait_for_server()
self.nodes = set()
self.edges = defaultdict(list)
self.distances = {}
with open(rospy.get_param("~waypoint_yaml"), 'r') as f:
self.waypointInfo = yaml.load(f)
for waypoint, info in self.waypointInfo.items():
self.add_node(waypoint)
for edge in info["edges"]:
self.add_edge(waypoint, edge, 1.0)
self.waypointInfo[waypoint]["position"]["x"] *= 0.555
self.waypointInfo[waypoint]["position"]["y"] *= 0.555
self.sub = rospy.Subscriber("/orb_slam/pose", PoseStamped,
self.pose_cb)
self._as.start()
rospy.loginfo("Started {}".format(name))
def preempt_cb(self, *args):
self.client.cancel_all_goals()
def add_node(self, value):
self.nodes.add(value)
def add_edge(self, from_node, to_node, distance):
self.edges[from_node].append(to_node)
# self.edges[to_node].append(from_node)
self.distances[(from_node, to_node)] = distance
def pose_cb(self, msg):
closest = (None, None)
for k, v in self.waypointInfo.items():
dist = self.get_dist(msg, v["position"])
if closest[0] is None or dist < closest[0]:
closest = (dist, k)
self.closest = closest[1]
def get_dist(self, pose1, position):
return np.sqrt(
np.power(pose1.pose.position.x - position["x"], 2) +
np.power(pose1.pose.position.y - position["y"], 2))
def execute_cb(self, goal):
path = self.shortest_path(self.closest, goal.waypoint)[1]
print path
for p in path:
if not self._as.is_preempt_requested():
rospy.loginfo("Going to waypoint: {}".format(p))
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = "map"
target.pose.position.x = self.waypointInfo[p]["position"]["x"]
target.pose.position.y = self.waypointInfo[p]["position"]["y"]
target.pose.orientation.x = self.waypointInfo[p][
"orientation"]["x"]
target.pose.orientation.y = self.waypointInfo[p][
"orientation"]["y"]
target.pose.orientation.z = self.waypointInfo[p][
"orientation"]["z"]
target.pose.orientation.w = self.waypointInfo[p][
"orientation"]["w"]
self.client.send_goal_and_wait(WayPointNavGoal(target))
if not self._as.is_preempt_requested():
self._as.set_succeeded(TopoNavResult())
else:
self._as.set_preempted()
def dijkstra(self, initial):
visited = {initial: 0}
path = {}
nodes = set(self.nodes)
while nodes:
min_node = None
for node in nodes:
if node in visited:
if min_node is None:
min_node = node
elif visited[node] < visited[min_node]:
min_node = node
if min_node is None:
break
nodes.remove(min_node)
current_weight = visited[min_node]
for edge in self.edges[min_node]:
weight = current_weight + self.distances[(min_node, edge)]
if edge not in visited or weight < visited[edge]:
visited[edge] = weight
path[edge] = min_node
return visited, path
def shortest_path(self, origin, destination):
visited, paths = self.dijkstra(origin)
full_path = deque()
_destination = paths[destination]
while _destination != origin:
full_path.appendleft(_destination)
_destination = paths[_destination]
full_path.appendleft(origin)
full_path.append(destination)
return visited[destination], list(full_path)
class MockNavigation():
def __init__(self, move_base_topic):
self.robot_pose_ = PoseStamped()
self.listener = tf.TransformListener()
self.navigation_succedes = True
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
self.move_base_as_ = SimpleActionServer(
move_base_topic,
MoveBaseAction,
execute_cb = self.move_base_cb,
auto_start = False)
self.move_base_as_.start()
def __del__(self):
self.move_base_as_.__del__()
def move_base_cb(self, goal):
rospy.loginfo('move_base_cb')
self.target_position = goal.target_pose.pose.position
self.target_orientation = goal.target_pose.pose.orientation
self.moves_base = True
while not self.reply:
rospy.sleep(0.2)
(trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
self.robot_pose_.pose.position.x = trans[0]
self.robot_pose_.pose.position.y = trans[1]
feedback = MoveBaseFeedback()
feedback.base_position.pose.position.x = \
self.robot_pose_.pose.position.x
feedback.base_position.pose.position.y = \
self.robot_pose_.pose.position.y
self.move_base_as_.publish_feedback(feedback)
if self.move_base_as_.is_preempt_requested():
self.preempted += 1
rospy.loginfo("Preempted!")
self.moves_base = False
self.move_base_as_.set_preempted()
return None
if self.move_base_as_.is_new_goal_available():
self.preempted += 1
self.move_base_cb(self.move_base_as_.accept_new_goal())
return None
else:
result = MoveBaseResult()
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
if self.navigation_succedes:
self.move_base_as_.set_succeeded(result)
else:
self.move_base_as_.set_aborted(result)
class LocalSearch():
VISUAL_FIELD_SIZE = 40
MIN_HEAD_ANGLE = -140
MAX_HEAD_ANGLE = 140
_feedback = LocalSearchFeedback()
_result = LocalSearchResult()
def __init__(self):
self._action_name = 'local_search'
self.found_marker = False
self.tracking_started = False
#initialize head mover
name_space = '/head_traj_controller/point_head_action'
self.head_client = SimpleActionClient(name_space, PointHeadAction)
self.head_client.wait_for_server()
rospy.loginfo('%s: Action client for PointHeadAction running' % self._action_name)
#initialize tracker mark
rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_tracker)
rospy.loginfo('%s: subscribed to AlvarMarkers' % self._action_name)
#initialize this client
self._as = SimpleActionServer(self._action_name, LocalSearchAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def marker_tracker(self, marker):
if self.tracking_started:
self.found_marker = True
rospy.loginfo('%s: marker found' % self._action_name)
def run(self, goal):
success = False
self.tracking_started = True
self.found_marker = False
rospy.loginfo('%s: Executing search for AR marker' % self._action_name)
# define a set of ranges to search
search_ranges = [
# first search in front of the robot
(0, self.VISUAL_FIELD_SIZE),
(self.VISUAL_FIELD_SIZE, -self.VISUAL_FIELD_SIZE),
# then search all directions
(-self.VISUAL_FIELD_SIZE, self.MAX_HEAD_ANGLE),
(self.MAX_HEAD_ANGLE, self.MIN_HEAD_ANGLE),
(self.MIN_HEAD_ANGLE, 0)
]
range_index = 0
#success = self.search_range(*(search_ranges[range_index]))
while (not success) and range_index < len(search_ranges) - 1:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Premepted' % self._action_name)
self._as.set_preempted()
break
range_index = range_index + 1
success = self.search_range(*(search_ranges[range_index]))
if success:
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded()
else:
self._as.set_aborted()
self.tracking_started = False
def search_range(self, start_angle, end_angle):
rospy.loginfo("{}: searching range {} {}".format(self._action_name, start_angle, end_angle))
angle_tick = self.VISUAL_FIELD_SIZE if (start_angle < end_angle) else -self.VISUAL_FIELD_SIZE
for cur_angle in xrange(start_angle, end_angle, angle_tick):
if self._as.is_preempt_requested():
return False
head_goal = self.lookat_goal(cur_angle)
rospy.loginfo('%s: Head move goal for %s: %s produced' % (self._action_name, str(cur_angle), str(head_goal)))
self.head_client.send_goal(head_goal)
self.head_client.wait_for_result(rospy.Duration.from_sec(5.0))
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head could not move to specified location')
break
# pause at each tick
rospy.sleep(0.3)
if (self.found_marker):
# found a marker!
return True
# no marker found
return False
def lookat_goal(self, angle):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = '/torso_lift_link'
head_goal.max_velocity = 0.8
angle_in_radians = math.radians(angle)
x = math.cos(angle_in_radians) * 5
y = math.sin(angle_in_radians) * 5
z = -0.5
head_goal.target.point = Point(x, y, z)
return head_goal
class Approach(object):
def __init__(self, name, takeoff_height):
self.robot_name = name
self.takeoff_height = takeoff_height
# Mutual exclusion
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
# Create trajectory server
self.trajectory_server = SimpleActionServer(
'approach_server', ExecuteDroneApproachAction, self.goCallback,
False)
self.server_feedback = ExecuteDroneApproachFeedback()
self.server_result = ExecuteDroneApproachResult()
# Get client from hector_quadrotor_actions
self.move_client = SimpleActionClient("/{}/action/pose".format(name),
PoseAction)
self.move_client.wait_for_server()
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("/{}/ground_truth/state".format(name), Odometry,
self.poseCallback)
# Subscribe to topic to receive the planned trajectory
rospy.Subscriber("/{}/move_group/display_planned_path".format(name),
DisplayTrajectory, self.planCallback)
#Auxiliary variables
self.trajectory = [] # Array with the trajectory to be executed
self.trajectory_received = False # Flag to signal trajectory received
self.odom_received = False # Flag to signal odom received
self.robot = RobotCommander(
robot_description="{}/robot_description".format(name),
ns="/{}".format(name))
self.display_trajectory_publisher = rospy.Publisher(
'/{}/move_group/display_planned_path'.format(name),
DisplayTrajectory,
queue_size=20)
# Variables for collision callback
self.validity_srv = rospy.ServiceProxy(
'/{}/check_state_validity'.format(name), GetStateValidity)
self.validity_srv.wait_for_service()
self.collision = False
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
#Start move_group
self.move_group = MoveGroup('earth', name)
self.move_group.set_planner(planner_id='RRTConnectkConfigDefault',
attempts=10,
allowed_time=2) #RRTConnectkConfigDefault
self.move_group.set_workspace([XMIN, YMIN, ZMIN, XMAX, YMAX,
ZMAX]) # Set the workspace size
# Get current robot position to define as start planning point
self.current_pose = self.robot.get_current_state()
#Start planningScenePublisher
self.scene_pub = PlanningScenePublisher(name, self.current_pose)
# Start trajectory server
self.trajectory_server.start()
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.notify()
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
# print(self.odometry)
self.odometry_me.release()
self.odom_received = True
def goCallback(self, pose):
'''
Require a plan to go to the desired target and try to execute it 5 time or return erro
'''
self.target = pose.goal
####################################################################
# First takeoff if the drone is close to ground
self.sonar_me.acquire()
while not (self.current_height and self.odometry):
self.sonar_me.wait()
if self.current_height < self.takeoff_height:
self.odometry_me.acquire()
takeoff_pose = PoseGoal()
takeoff_pose.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
takeoff_pose.target_pose.pose.position.x = self.odometry.position.x
takeoff_pose.target_pose.pose.position.y = self.odometry.position.y
takeoff_pose.target_pose.pose.position.z = self.odometry.position.z + self.takeoff_height - self.current_height #Add the heght error to the height
takeoff_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
takeoff_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
takeoff_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
takeoff_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.odometry_me.release()
self.move_client.send_goal(takeoff_pose)
self.move_client.wait_for_result()
result = self.move_client.get_state()
if result == GoalStatus.ABORTED:
rospy.logerr("Abort approach! Unable to execute takeoff!")
self.trajectory_server.set_aborted()
return
self.sonar_me.release()
####################################################################
rospy.loginfo("Try to start from [{},{},{}]".format(
self.odometry.position.x, self.odometry.position.y,
self.odometry.position.z))
rospy.loginfo("Try to go to [{},{},{}]".format(self.target.position.x,
self.target.position.y,
self.target.position.z))
self.trials = 0
while self.trials < 5:
rospy.logwarn("Attempt {}".format(self.trials + 1))
if (self.trials > 1):
self.target.position.z += 2 * rd() - 1
result = self.go(self.target)
if (result == 'replan') or (result == 'no_plan'):
self.trials += 1
else:
self.trials = 10
self.collision = False
if result == 'ok':
self.trajectory_server.set_succeeded()
elif (result == 'preempted'):
self.trajectory_server.set_preempted()
else:
self.trajectory_server.set_aborted()
def go(self, target_):
'''
Function to plan and execute the trajectory one time
'''
# Insert goal position on an array
target = []
target.append(target_.position.x)
target.append(target_.position.y)
target.append(target_.position.z)
target.append(target_.orientation.x)
target.append(target_.orientation.y)
target.append(target_.orientation.z)
target.append(target_.orientation.w)
#Define target for move_group
# self.move_group.set_joint_value_target(target)
self.move_group.set_target(target)
self.odometry_me.acquire()
self.current_pose.multi_dof_joint_state.transforms[
0].translation.x = self.odometry.position.x
self.current_pose.multi_dof_joint_state.transforms[
0].translation.y = self.odometry.position.y
self.current_pose.multi_dof_joint_state.transforms[
0].translation.z = self.odometry.position.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.x
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.y
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.w
self.odometry_me.release()
#Set start state
self.move_group.set_start_state(self.current_pose)
# Plan a trajectory till the desired target
plan = self.move_group.plan()
if plan.planned_trajectory.multi_dof_joint_trajectory.points: # Execute only if has points on the trajectory
while (not self.trajectory_received):
rospy.loginfo("Waiting for trajectory!")
rospy.sleep(0.2)
# rospy.loginfo("TRAJECTORY: {}".format(self.trajectory))
#Execute trajectory with action_pose
last_pose = self.trajectory[0]
for pose in self.trajectory:
# Verify preempt call
if self.trajectory_server.is_preempt_requested():
self.move_client.send_goal(last_pose)
self.move_client.wait_for_result()
self.scene_pub.publishScene()
self.trajectory_received = False
self.odom_received = False
return 'preempted'
#Send next pose to move
self.next_pose = pose.target_pose.pose
self.move_client.send_goal(pose,
feedback_cb=self.collisionCallback)
self.move_client.wait_for_result()
result = self.move_client.get_state()
self.scene_pub.publishScene()
# Abort if the drone can not reach the position
if result == GoalStatus.ABORTED:
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'aborted'
elif result == GoalStatus.PREEMPTED:
# last_pose.target_pose.pose.position.z += rd() - 0.5
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'replan'
elif result == GoalStatus.SUCCEEDED:
self.trials = 0
last_pose = pose
self.server_feedback.current_pose = self.odometry
self.trajectory_server.publish_feedback(self.server_feedback)
# Reset control variables
self.trajectory_received = False
self.odom_received = False
rospy.loginfo("Trajectory is traversed!")
return 'ok'
else:
rospy.logerr("Trajectory is empty. Planning was unsuccessful.")
return 'no_plan'
def planCallback(self, msg):
'''
Receive planned trajectories and insert it into an array of waypoints
'''
if (not self.odom_received):
return
# Variable to calculate the distance difference between 2 consecutive points
last_pose = PoseGoal()
last_pose.target_pose.pose.position.x = self.odometry.position.x
last_pose.target_pose.pose.position.y = self.odometry.position.y
last_pose.target_pose.pose.position.z = self.odometry.position.z
last_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
last_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
last_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
last_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.trajectory = []
last_motion_theta = 0
for t in msg.trajectory:
for point in t.multi_dof_joint_trajectory.points:
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
# Orientate the robot always to the motion direction
delta_x = point.transforms[
0].translation.x - last_pose.target_pose.pose.position.x
delta_y = point.transforms[
0].translation.y - last_pose.target_pose.pose.position.y
motion_theta = atan2(delta_y, delta_x)
last_motion_theta = motion_theta
# Make the robot orientation fit with the motion orientation if the movemente on xy is bigger than RESOLUTION
# if (abs(delta_x) > RESOLUTION) or (abs(delta_y) > RESOLUTION):
q = quaternion_from_euler(0, 0, motion_theta)
waypoint.target_pose.pose.orientation.x = q[0]
waypoint.target_pose.pose.orientation.y = q[1]
waypoint.target_pose.pose.orientation.z = q[2]
waypoint.target_pose.pose.orientation.w = q[3]
# else:
# waypoint.target_pose.pose.orientation.x = point.transforms[0].rotation.x
# waypoint.target_pose.pose.orientation.y = point.transforms[0].rotation.y
# waypoint.target_pose.pose.orientation.z = point.transforms[0].rotation.z
# waypoint.target_pose.pose.orientation.w = point.transforms[0].rotation.w
# Add a rotation inplace if next position has an angle difference bigger than 45
if abs(motion_theta - last_motion_theta) > 0.785:
last_pose.target_pose.pose.orientation = waypoint.target_pose.pose.orientation
self.trajectory.append(last_pose)
last_pose = copy.copy(
waypoint) # Save pose to calc the naxt delta
last_motion_theta = motion_theta
self.trajectory.append(waypoint)
#Insert a last point to ensure that the robot end at the right position
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
waypoint.target_pose.pose.orientation.x = point.transforms[
0].rotation.x
waypoint.target_pose.pose.orientation.y = point.transforms[
0].rotation.y
waypoint.target_pose.pose.orientation.z = point.transforms[
0].rotation.z
waypoint.target_pose.pose.orientation.w = point.transforms[
0].rotation.w
self.trajectory.append(waypoint)
self.trajectory_received = True
def collisionCallback(self, feedback):
'''
This callback runs on every feedback message received
'''
validity_msg = GetStateValidityRequest(
) # Build message to verify collision
validity_msg.group_name = PLANNING_GROUP
if self.next_pose and (not self.collision):
self.odometry_me.acquire()
x = self.odometry.position.x
y = self.odometry.position.y
z = self.odometry.position.z
# Distance between the robot and the next position
dist = sqrt((self.next_pose.position.x - x)**2 +
(self.next_pose.position.y - y)**2 +
(self.next_pose.position.z - z)**2)
# Pose to verify collision
pose = Transform()
pose.rotation.x = self.odometry.orientation.x
pose.rotation.y = self.odometry.orientation.y
pose.rotation.z = self.odometry.orientation.z
pose.rotation.w = self.odometry.orientation.w
self.odometry_me.release()
#Verify possible collisions on diferent points between the robot and the goal point
# rospy.logerr("\n\n\nCOLLISION CALLBACK: ")
# rospy.logerr(dist)
for d in arange(RESOLUTION, dist + 0.5, RESOLUTION):
pose.translation.x = (self.next_pose.position.x -
x) * (d / dist) + x
pose.translation.y = (self.next_pose.position.y -
y) * (d / dist) + y
pose.translation.z = (self.next_pose.position.z -
z) * (d / dist) + z
self.current_pose.multi_dof_joint_state.transforms[
0] = pose # Insert the correct odometry value
validity_msg.robot_state = self.current_pose
# Call service to verify collision
collision_res = self.validity_srv.call(validity_msg)
# print("\nCollision response:")
# print(collision_res)
# Check if robot is in collision
if not collision_res.valid:
# print(validity_msg)
rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format(
pose.translation.x, pose.translation.y,
pose.translation.z))
rospy.logerr('Current pose [x:{} y:{} z:{}]'.format(
x, y, z))
# print(collision_res)
self.move_client.cancel_goal()
self.collision = True
return
class SmartArmGripperActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 2 # Number of joints to manage
self.ERROR_THRESHOLD = 0.01 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(5.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME, anonymous=True)
# Initialize publisher & subscriber for left finger
self.left_finger_frame = 'arm_left_finger_link'
self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64)
rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger)
rospy.wait_for_message('finger_left_controller/state', JointControllerState)
# Initialize publisher & subscriber for right finger
self.right_finger_frame = 'arm_right_finger_link'
self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64)
rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger)
rospy.wait_for_message('finger_right_controller/state', JointControllerState)
# Initialize action server
self.result = SmartArmGripperResult()
self.feedback = SmartArmGripperFeedback()
self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback)
# Reset gripper position
rospy.sleep(1)
self.reset_gripper_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def reset_gripper_position(self):
self.left_finger_pub.publish(0.0)
self.right_finger_pub.publish(0.0)
rospy.sleep(5)
def read_left_finger(self, data):
self.left_finger = data
self.has_latest_left_finger = True
def read_right_finger(self, data):
self.right_finger = data
self.has_latest_right_finger = True
def wait_for_latest_controller_states(self, timeout):
self.has_latest_left_finger = False
self.has_latest_right_finger = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_left_finger == False or self.has_latest_right_finger == False) and \
(rospy.Time.now() - start < timeout):
r.sleep()
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
rospy.loginfo("%s: Executing move gripper", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
rospy.loginfo("%s: Aborted: Invalid Goal", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal to controllers
self.left_finger_pub.publish(target_joints[0])
self.right_finger_pub.publish(target_joints[1])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.left_finger.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.right_finger.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current gripper position as feedback
self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server.set_succeeded(self.result)
class MotionService(object):
def __init__(self):
rospy.init_node('motion_service')
# Load config
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
# Create publisher for first target
if len(config_loader.targets) > 0:
self._motion_publisher = MotionPublisher(
config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix)
else:
rospy.logerr('Robot config needs to contain at least one valid target.')
self._motion_data = MotionData(config_loader.robot_config)
# Subscriber to start motions via topic
self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb)
# Create action server
self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False)
self._action_server.register_goal_callback(self._execute_motion_goal)
self._action_server.register_preempt_callback(self._preempt_motion_goal)
self._preempted = False
def _execute_motion_goal(self):
goal = self._action_server.accept_new_goal()
# Check if motion exists
if goal.motion_name not in self._motion_data:
result = ExecuteMotionResult()
result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN]
result.error_string = "The requested motion is unknown."
self._action_server.set_aborted(result)
return
# check if a valid time_factor is set, otherwise default to 1.0
if goal.time_factor <= 0.0:
goal.time_factor = 1.0
self._preempted = False
self._motion_publisher.publish_motion(self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished)
print '[MotionService] New action goal received.'
def _preempt_motion_goal(self):
self._motion_publisher.stop_motion()
print '[MotionService] Preempting goal.'
self._preempted = True
def _trajectory_finished(self, error_codes, error_groups):
result = ExecuteMotionResult()
result.error_code = error_codes
error_string = ""
for error_code, error_group in zip(error_codes, error_groups):
error_string += error_group + ': ' + str(error_code) + '\n'
result.error_string = error_string
if self._preempted:
self._action_server.set_preempted(result)
else:
self._action_server.set_succeeded(result)
print '[MotionService] Trajectory finished with error code: \n', error_string
def _execute_motion(self, request):
response = ExecuteMotionResponse()
# check if a motion with this name exists
if request.motion_name not in self._motion_data:
print '[MotionService] Unknown motion name: "%s"' % request.motion_name
response.ok = False
response.finish_time.data = rospy.Time.now()
return response
# check if a valid time_factor is set, otherwise default to 1.0
if request.time_factor <= 0.0:
request.time_factor = 1.0
# find the duration of the requested motion
motion_duration = 0.0
for poses in self._motion_data[request.motion_name].values():
if len(poses) > 0:
endtime = poses[-1]['starttime'] + poses[-1]['duration']
motion_duration = max(motion_duration, endtime)
motion_duration *= request.time_factor
# execute motion
print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (request.motion_name, request.time_factor, motion_duration)
self._motion_publisher.publish_motion(self._motion_data[request.motion_name], request.time_factor)
# reply with ok and the finish_time of this motion
response.ok = True
response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(motion_duration)
return response
def start_server(self):
rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion, self._execute_motion)
self._action_server.start()
print '[MotionService] Waiting for calls...'
rospy.spin()
def motion_command_request_cb(self, command):
print "[MotionService] Initiate motion command via topic ..."
request = ExecuteMotion()
request.motion_name = command.motion_name
request.time_factor = command.time_factor
self._execute_motion(request)
print "[MotionService] Motion command complete"
class ShakePitchObjectActionServer:
def __init__(self):
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording)
self.wrist_pitch_velocity_srv = rospy.ServiceProxy('/wrist_pitch_controller/set_velocity', SetVelocity)
self.wrist_pitch_command_pub = rospy.Publisher('wrist_pitch_controller/command', Float64)
self.action_server = SimpleActionServer(ACTION_NAME,
ShakePitchObjectAction,
execute_cb=self.shake_pitch_object,
auto_start=False)
def initialize(self):
rospy.loginfo('%s: waiting for wubble_grasp_status service' % ACTION_NAME)
rospy.wait_for_service('/wubble_grasp_status')
rospy.loginfo('%s: connected to wubble_grasp_status service' % ACTION_NAME)
rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME)
rospy.wait_for_service('audio_dump/start_audio_recording')
rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME)
rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME)
rospy.wait_for_service('audio_dump/stop_audio_recording')
rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME)
rospy.loginfo('%s: waiting for wrist_pitch_controller service' % ACTION_NAME)
rospy.wait_for_service('/wrist_pitch_controller/set_velocity')
rospy.loginfo('%s: connected to wrist_pitch_controller service' % ACTION_NAME)
self.action_server.start()
def shake_pitch_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
wrist_pitch_state = '/wrist_pitch_controller/state'
desired_velocity = 6.0
distance = 1.0
threshold = 0.1
timeout = 2.0
try:
msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
current_pos = msg.position
start_pos = current_pos
# set wrist to initial position
self.wrist_pitch_velocity_srv(3.0)
self.wrist_pitch_command_pub.publish(distance)
end_time = rospy.Time.now() + rospy.Duration(timeout)
while current_pos < distance-threshold and rospy.Time.now() < end_time:
msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
current_pos = msg.position
rospy.sleep(10e-3)
self.wrist_pitch_velocity_srv(desired_velocity)
# start recording sound and shaking
self.start_audio_recording_srv(InfomaxAction.SHAKE_PITCH, goal.category_id)
rospy.sleep(0.5)
for i in range(2):
self.wrist_pitch_command_pub.publish(-distance)
end_time = rospy.Time.now() + rospy.Duration(timeout)
while current_pos > -distance+threshold and rospy.Time.now() < end_time:
msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
current_pos = msg.position
rospy.sleep(10e-3)
self.wrist_pitch_command_pub.publish(distance)
end_time = rospy.Time.now() + rospy.Duration(timeout)
while current_pos < distance-threshold and rospy.Time.now() < end_time:
msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
current_pos = msg.position
rospy.sleep(10e-3)
rospy.sleep(0.5)
# check if are still holding an object after shaking
sound_msg = None
grasp_status = self.get_grasp_status_srv()
if grasp_status.is_hand_occupied:
sound_msg = self.stop_audio_recording_srv(True)
else:
self.stop_audio_recording_srv(False)
# reset wrist to starting position
self.wrist_pitch_velocity_srv(3.0)
self.wrist_pitch_command_pub.publish(start_pos)
end_time = rospy.Time.now() + rospy.Duration(timeout)
while current_pos < distance-threshold and rospy.Time.now() < end_time:
msg = rospy.wait_for_message(wrist_pitch_state, DynamixelJointState, timeout)
current_pos = msg.position
rospy.sleep(10e-3)
rospy.sleep(0.5)
if sound_msg:
self.action_server.set_succeeded(ShakePitchObjectResult(sound_msg.recorded_sound))
return
else:
self.action_server.set_aborted()
return
except:
rospy.logerr('%s: attempted pitch failed - %s' % ACTION_NAME)
self.stop_audio_recording_srv(False)
self.action_server.set_aborted()
class RobbieArmActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 5 # Number of joints to manage
self.ERROR_THRESHOLD = 0.15 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME + 'server', anonymous=True)
# Initialize publisher & subscriber for shoulder pan
self.shoulder_pan_frame = 'arm_shoulder_tilt_link'
self.shoulder_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64)
rospy.Subscriber('shoulder_pan_controller/state', JointState, self.read_shoulder_pan)
rospy.wait_for_message('shoulder_pan_controller/state', JointState)
# Initialize publisher & subscriber for arm tilt
self.arm_tilt_frame = 'arm_pan_tilt_bracket'
self.arm_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.arm_tilt_pub = rospy.Publisher('arm_tilt_controller/command', Float64)
rospy.Subscriber('arm_tilt_controller/state', JointState, self.read_arm_tilt)
rospy.wait_for_message('arm_tilt_controller/state', JointState)
# Initialize publisher & subscriber for elbow tilt
self.elbow_tilt_frame = 'arm_bracket'
#self.elbow_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64)
rospy.Subscriber('elbow_tilt_controller/state', JointState, self.read_elbow_tilt)
rospy.wait_for_message('elbow_tilt_controller/state', JointState)
# Initialize publisher & subscriber for wrist pan
self.wrist_pan_frame = 'wrist_pan_link'
self.wrist_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_pan_pub = rospy.Publisher('wrist_pan_controller/command', Float64)
rospy.Subscriber('wrist_pan_controller/state', JointState, self.read_wrist_pan)
rospy.wait_for_message('wrist_pan_controller/state', JointState)
# Initialize publisher & subscriber for wrist tilt
self.wrist_tilt_frame = 'wrist_tilt_link'
self.wrist_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_tilt_pub = rospy.Publisher('wrist_tilt_controller/command', Float64)
rospy.Subscriber('wrist_tilt_controller/state', JointState, self.read_wrist_tilt)
rospy.wait_for_message('wrist_tilt_controller/state', JointState)
# Initialize tf listener
self.tf = tf.TransformListener()
# Initialize joints action server
self.result = RobbieArmResult()
self.feedback = RobbieArmFeedback()
self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server = SimpleActionServer(NAME, RobbieArmAction, self.execute_callback)
# Reset arm position
rospy.sleep(1)
self.reset_arm_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def reset_arm_position(self):
# reset arm to cobra position
self.shoulder_pan_pub.publish(0.0)
self.arm_tilt_pub.publish(1.572222)
self.elbow_tilt_pub.publish(-1.572222)
self.wrist_pan_pub.publish(0.0)
self.wrist_tilt_pub.publish(0.0)
rospy.sleep(12)
def read_shoulder_pan(self, pan_data):
self.shoulder_pan = pan_data
self.has_latest_shoulder_pan = True
def read_arm_tilt(self, tilt_data):
self.arm_tilt = tilt_data
self.has_latest_arm_tilt = True
def read_elbow_tilt(self, tilt_data):
self.elbow_tilt = tilt_data
self.has_latest_elbow_tilt = True
def read_wrist_pan(self, rotate_data):
self.wrist_pan = rotate_data
self.has_latest_wrist_pan = True
def read_wrist_tilt(self, rotate_data):
self.wrist_tilt = rotate_data
self.has_latest_wrist_tilt = True
def wait_for_latest_controller_states(self, timeout):
self.has_latest_shoulder_pan = False
self.has_latest_arm_tilt = False
self.has_latest_elbow_tilt = False
self.has_latest_wrist_pan = False
self.has_latest_wrist_tilt = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_shoulder_pan == False or self.has_latest_arm_tilt == False or \
self.has_latest_elbow_tilt == False or self.has_latest_wrist_tilt == False or self.has_latest_wrist_pan == False) and \
(rospy.Time.now() - start < timeout):
r.sleep()
def transform_target_point(self, point):
rospy.loginfo("%s: Retrieving IK solutions", NAME)
rospy.wait_for_service('smart_arm_ik_service', 10)
ik_service = rospy.ServiceProxy('smart_arm_ik_service', SmartArmIK)
resp = ik_service(point)
if (resp and resp.success):
return resp.solutions[0:4]
else:
raise Exception, "Unable to obtain IK solutions."
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
rospy.loginfo("%s: Executing move arm", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
try:
# Convert target point to target joints (find an IK solution)
target_joints = self.transform_target_point(goal.target_point)
except (Exception, tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("%s: Aborted: IK Transform Failure", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal to controllers
self.shoulder_pan_pub.publish(target_joints[0])
self.arm_tilt_pub.publish(target_joints[1])
self.elbow_tilt_pub.publish(target_joints[2])
self.wrist_pan_pub.publish(target_joints[3])
self.wrist_tilt_pub.publish(target_joints[4])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.shoulder_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.arm_tilt.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[2] - self.elbow_tilt.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[3] - self.wrist_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[4] - self.wrist_tilt.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current arm position as feedback
self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server.set_succeeded(self.result)
class RobbieHeadActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 2 # Number of joints to manage
self.ERROR_THRESHOLD = 0.02 # Report success if error reaches below threshold (0.015 also works)
self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME, anonymous=True)
# Initialize publisher & subscriber for pan
self.head_pan_frame = 'head_pan_link'
self.head_pan = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.head_pan_pub = rospy.Publisher('head_pan_controller/command', Float64)
rospy.Subscriber('head_pan_controller/state_pr2_msgs', JointControllerState, self.read_current_pan)
rospy.wait_for_message('head_pan_controller/state_pr2_msgs', JointControllerState)
# Initialize publisher & subscriber for tilt
self.head_tilt_frame = 'head_tilt_link'
self.head_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.head_tilt_pub = rospy.Publisher('head_tilt_controller/command', Float64)
rospy.Subscriber('head_tilt_controller/state_pr2_msgs', JointControllerState, self.read_current_tilt)
rospy.wait_for_message('head_tilt_controller/state_pr2_msgs', JointControllerState)
# Initialize tf listener
self.tf = tf.TransformListener()
# Initialize point action server
self.result = RobbieHeadResult()
self.feedback = RobbieHeadFeedback()
self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
self.server = SimpleActionServer(NAME, RobbieHeadAction, self.execute_callback, auto_start=False)
# Reset head position
rospy.sleep(1)
self.reset_head_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def read_current_pan(self, pan_data):
self.head_pan = pan_data
self.has_latest_pan = True
def read_current_tilt(self, tilt_data):
self.head_tilt = tilt_data
self.has_latest_tilt = True
def reset_head_position(self):
self.head_pan_pub.publish(0.0)
self.head_tilt_pub.publish(0.0)
rospy.sleep(5)
def wait_for_latest_controller_states(self, timeout):
self.has_latest_pan = False
self.has_latest_tilt = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_pan == False or self.has_latest_tilt == False) and (rospy.Time.now() - start < timeout):
r.sleep()
def transform_target_point(self, point):
pan_target_frame = self.head_pan_frame
tilt_target_frame = self.head_tilt_frame
# Wait for tf info (time-out in 5 seconds)
self.tf.waitForTransform(pan_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
self.tf.waitForTransform(tilt_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
# Transform target point to pan reference frame & retrieve the pan angle
pan_target = self.tf.transformPoint(pan_target_frame, point)
pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)
#rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \
# NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle)
# Transform target point to tilt reference frame & retrieve the tilt angle
tilt_target = self.tf.transformPoint(tilt_target_frame, point)
tilt_angle = math.atan2(tilt_target.point.z,
math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))
#rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \
# NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle)
return [pan_angle, tilt_angle]
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
rospy.loginfo("%s: Executing move head", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
try:
# Try to convert target point to pan & tilt angles
target_joints = self.transform_target_point(goal.target_point)
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("%s: Aborted: Transform Failure", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal command to controllers
self.head_pan_pub.publish(target_joints[0])
self.head_tilt_pub.publish(target_joints[1])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.head_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.head_tilt.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current head position as feedback
self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
self.server.set_succeeded(self.result)
class WubbleGripperGraspController:
def __init__(self):
self.object_presence_pressure_threshold = rospy.get_param('object_presence_pressure_threshold', 200.0)
self.object_presence_opening_threshold = rospy.get_param('object_presence_opening_threshold', 0.02)
gripper_action_name = rospy.get_param('gripper_action_name', 'wubble_gripper_command_action')
self.gripper_action_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction)
#
# while not rospy.is_shutdown():
# try:
# self.gripper_action_client.wait_for_server(timeout=rospy.Duration(2.0))
# break
# except ROSException as e:
# rospy.loginfo('Waiting for %s action' % gripper_action_name)
# except:
# rospy.logerr('Unexpected error')
# raise
rospy.loginfo('Using gripper action client on topic %s' % gripper_action_name)
query_service_name = rospy.get_param('grasp_query_name', 'wubble_grasp_status')
self.query_srv = rospy.Service(query_service_name, GraspStatus, self.process_grasp_status)
posture_action_name = rospy.get_param('posture_action_name', 'wubble_gripper_grasp_action')
self.action_server = SimpleActionServer(posture_action_name, GraspHandPostureExecutionAction, self.process_grasp_action, False)
self.action_server.start()
rospy.loginfo('wubble_gripper grasp hand posture action server started on topic %s' % posture_action_name)
def process_grasp_action(self, msg):
gripper_command = WubbleGripperGoal()
if msg.goal == GraspHandPostureExecutionGoal.GRASP:
rospy.loginfo('Received GRASP request')
# if not msg.goal.grasp.position:
# msg = 'wubble gripper grasp execution: position vector empty in requested grasp'
# rospy.logerr(msg)
# self.action_server.set_aborted(text=msg)
# return
#
gripper_command.command = WubbleGripperGoal.CLOSE_GRIPPER
gripper_command.torque_limit = 0.4
gripper_command.dynamic_torque_control = True
gripper_command.pressure_upper = 1900.0
gripper_command.pressure_lower = 1800.0
elif msg.goal == GraspHandPostureExecutionGoal.PRE_GRASP:
rospy.loginfo('Received PRE_GRASP request')
gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER
gripper_command.torque_limit = 0.6
elif msg.goal == GraspHandPostureExecutionGoal.RELEASE:
rospy.loginfo('Received RELEASE request')
gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER
gripper_command.torque_limit = 0.6
else:
msg = 'wubble gripper grasp execution: unknown goal code (%d)' % msg.goal
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
self.gripper_action_client.send_goal(gripper_command)
self.gripper_action_client.wait_for_result()
self.action_server.set_succeeded()
def process_grasp_status(self, msg):
result = GraspStatus()
pressure_msg = rospy.wait_for_message('/total_pressure', Float64)
opening_msg = rospy.wait_for_message('/gripper_opening', Float64)
left_pos = rospy.wait_for_message('/left_finger_controller/state', JointState).position
right_pos = rospy.wait_for_message('/right_finger_controller/state', JointState).position
if pressure_msg.data <= self.object_presence_pressure_threshold:
rospy.loginfo('Gripper grasp query false: gripper total pressure is below threshold (%.2f <= %.2f)' % (pressure_msg.data, self.object_presence_pressure_threshold))
return False
else:
if opening_msg.data <= self.object_presence_opening_threshold:
rospy.loginfo('Gripper grasp query false: gripper opening is below threshold (%.2f <= %.2f)' % (opening_msg.data, self.object_presence_opening_threshold))
return False
else:
if (left_pos > 0.85 and right_pos > 1.10) or (right_pos < -0.85 and left_pos < -1.10):
rospy.loginfo('Gripper grasp query false: gripper fingers are too off center (%.2f, %.2f)' % (left_pos, right_pos))
return False
rospy.loginfo('Gripper grasp query true: pressure is %.2f, opening is %.2f' % (pressure_msg.data, opening_msg.data))
return True
class BebopActionServer(object):
def __init__(self):
# Create all the ActionServers.
self.as_land = SimpleActionServer("BebopLandAction",
BebopLandAction,
execute_cb=self.cb_land,
auto_start=False)
self.as_load = SimpleActionServer("BebopLoadAction",
BebopLoadAction,
execute_cb=self.cb_load,
auto_start=False)
self.as_move = SimpleActionServer("BebopMoveBaseAction",
BebopMoveBaseAction,
execute_cb=self.cb_move_base,
auto_start=False)
self.as_takeoff = SimpleActionServer("BebopTakeOffAction",
BebopTakeOffAction,
execute_cb=self.cb_takeoff,
auto_start=False)
self.as_unload = SimpleActionServer("BebopUnloadAction",
BebopUnloadAction,
execute_cb=self.cb_unload,
auto_start=False)
self.as_follow = SimpleActionServer("BebopFollowAction",
BebopFollowAction,
execute_cb=self.cb_follow,
auto_start=False)
# Frequency for controller feedback loop
self.loop_rate = rospy.Rate(10)
# Setup controller
if rospy.get_param('~test_mode', False):
rospy.loginfo("%s running", "testmode")
self.controller = TestController()
else:
if 'DRONESIM' in os.environ or rospy.get_param('~sim', False):
self.controller = ARDroneSimController(
rospy.names.get_namespace()[1:-1])
else:
self.controller = BebopController(
rospy.names.get_namespace()[0:-1])
# self.controller = BebopController('/bebop')
self.controller.set_mode('auto')
# Finally, start action servers
self.as_land.start()
self.as_load.start()
self.as_move.start()
self.as_takeoff.start()
self.as_unload.start()
self.as_follow.start()
rospy.loginfo("%s running", rospy.get_name())
# Loop if manual mode is set waiting for automatic to be reissued
def wait_manual_mode(self, action, goal):
if self.controller.isManualMode():
print("Manual mode active, wait to apply action {}".format(action))
while self.controller.isManualMode() and not rospy.is_shutdown():
self.loop_rate.sleep()
def spin(self):
self.controller.run()
# rospy.spin()
def cb_load(self, goal):
self.wait_manual_mode('load', goal)
rospy.loginfo("/BebopActionServer/cb_load action_id %s",
self.as_load.current_goal.get_goal_id().id)
self.mark_load_event(self.as_load)
def cb_land(self, goal):
self.wait_manual_mode('land', goal)
rospy.loginfo("/BebopActionServer/cb_land action_id %s",
self.as_land.current_goal.get_goal_id().id)
self.controller.land()
rospy.sleep(1)
self.handle_feedback(self.as_land)
def cb_follow(self, goal):
self.wait_manual_mode('follow', goal)
rospy.loginfo("/BebopActionServer/cb_follow action_id %s",
self.as_follow.current_goal.get_goal_id().id)
if not self.controller.isManulMode():
self.as_follow.set_succeeded()
def cb_unload(self, goal):
self.wait_manual_mode('unload', goal)
rospy.loginfo("/BebopActionServer/cb_unload action_id %s",
self.as_unload.current_goal.get_goal_id().id)
self.mark_load_event(self.as_unload)
def cb_takeoff(self, goal):
self.wait_manual_mode('takeoff', goal)
rospy.loginfo("/BebopActionServer/cb_takeoff action_id %s",
self.as_takeoff.current_goal.get_goal_id().id)
self.controller.takeoff()
self.handle_feedback(self.as_takeoff)
def cb_move_base(self, goal):
self.wait_manual_mode('move', goal)
if not self.controller.airborne():
self.controller.takeoff()
status, preempted = self.handle_feedback(self.as_move,
send_result=False)
rospy.loginfo("/BebopActionServer/cb_move_base action_id %s",
self.as_move.current_goal.get_goal_id().id)
point_goal = PointStamped()
point_goal.header = goal.target_pose.header
point_goal.point = goal.target_pose.pose.position
self.controller.set_goal(point_goal)
rospy.sleep(1)
self.handle_feedback(self.as_move)
def mark_load_event(self, actionserver):
rospy.loginfo("Load action: Set yaw")
self.controller.set_yaw(0)
status, preempted = self.handle_feedback(actionserver,
send_result=False)
rospy.loginfo("Load action: Rotate down")
self.controller.set_yaw(3)
self.controller.set_height(0.8)
status, preempted = self.handle_feedback(actionserver,
send_result=False)
if status == ActionStatus.COMPLETED:
rospy.loginfo("Load action: Rotate up")
self.controller.set_height(self.controller.setpoint_height)
self.controller.set_yaw(0)
self.handle_feedback(actionserver, send_result=True)
else:
self.send_result(actionserver, status, preempted)
def handle_feedback(self, actionserver, send_result=True):
preempted = False
while self.controller.get_action_status() <= ActionStatus.STARTED:
if rospy.is_shutdown():
self.controller.abort_action()
sys.exit()
elif actionserver.is_preempt_requested():
preempted = True
self.controller.abort_action()
self.loop_rate.sleep()
status = self.controller.get_action_status()
if send_result:
# If operator interrupted then we expect the operator to leave the drone in a
# good state. The plannner will send a new action which will wait until
# automatic mode is reissued.
if status == ActionStatus.FAILED and self.controller.isManualMode(
):
status = ActionStatus.COMPLETED
self.send_result(actionserver, status, preempted)
return status, preempted
def send_result(self, actionserver, status, preempted):
if status == ActionStatus.COMPLETED:
actionserver.set_succeeded()
else:
if preempted:
actionserver.set_preempted()
else:
actionserver.set_aborted()
class PlaceObjectActionServer:
def __init__(self):
self.start_audio_recording_srv = rospy.ServiceProxy('/audio_dump/start_audio_recording', StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy('/audio_dump/stop_audio_recording', StopAudioRecording)
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction)
self.attached_object_pub = rospy.Publisher('/attached_collision_object', AttachedCollisionObject)
self.action_server = SimpleActionServer(ACTION_NAME,
PlaceObjectAction,
execute_cb=self.place_object,
auto_start=False)
def initialize(self):
rospy.loginfo('%s: waiting for audio_dump/start_audio_recording service' % ACTION_NAME)
rospy.wait_for_service('audio_dump/start_audio_recording')
rospy.loginfo('%s: connected to audio_dump/start_audio_recording service' % ACTION_NAME)
rospy.loginfo('%s: waiting for audio_dump/stop_audio_recording service' % ACTION_NAME)
rospy.wait_for_service('audio_dump/stop_audio_recording')
rospy.loginfo('%s: connected to audio_dump/stop_audio_recording service' % ACTION_NAME)
rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' % ACTION_NAME)
self.posture_controller.wait_for_server()
rospy.loginfo('%s: connected to wubble_gripper_grasp_action' % ACTION_NAME)
rospy.loginfo('%s: waiting for move_left_arm action server' % ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server' % ACTION_NAME)
self.action_server.start()
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
def place_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
# check that we have something in hand before placing it
grasp_status = self.get_grasp_status_srv()
# if the object is still in hand after lift is done we are good
if not grasp_status.is_hand_occupied:
rospy.logerr('%s: gripper empty, nothing to place' % ACTION_NAME)
self.action_server.set_aborted()
return
gripper_paddings = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
# disable collisions between attached object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.01
# disable collisions between arm and table
collision_operation3 = CollisionOperation()
collision_operation3.object1 = collision_support_surface_name
collision_operation3.object2 = ARM_GROUP_NAME
collision_operation3.operation = CollisionOperation.DISABLE
collision_operation3.penetration_distance = 0.01
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2, collision_operation3]
self.start_audio_recording_srv(InfomaxAction.PLACE, goal.category_id)
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
PLACE_POSITION,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
sound_msg = None
grasp_status = self.get_grasp_status_srv()
self.open_gripper()
rospy.sleep(0.5)
# if after lowering arm gripper is still holding an object life's good
if grasp_status.is_hand_occupied:
sound_msg = self.stop_audio_recording_srv(True)
else:
self.stop_audio_recording_srv(False)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
if sound_msg:
self.action_server.set_succeeded(PlaceObjectResult(sound_msg.recorded_sound))
return
else:
self.action_server.set_aborted()
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted place failed' % ACTION_NAME)
self.action_server.set_aborted()
class PutDownAtActionServer:
def __init__(self):
self.start_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/start_audio_recording', StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/stop_audio_recording', StopAudioRecording)
self.grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning',
GraspPlanning)
self.get_solver_info_srv = rospy.ServiceProxy(
'/wubble2_left_arm_kinematics/get_ik_solver_info',
GetKinematicSolverInfo)
self.get_ik_srv = rospy.ServiceProxy(
'/wubble2_left_arm_kinematics/get_ik', GetPositionIK)
self.set_planning_scene_diff_client = rospy.ServiceProxy(
'/environment_server/set_planning_scene_diff',
SetPlanningSceneDiff)
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status',
GraspStatus)
self.find_cluster_bbox = rospy.ServiceProxy(
'/find_cluster_bounding_box', FindClusterBoundingBox)
self.posture_controller = SimpleActionClient(
'/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.move_arm_client = SimpleActionClient('/move_left_arm',
MoveArmAction)
self.attached_object_pub = rospy.Publisher(
'/attached_collision_object', AttachedCollisionObject)
self.action_server = SimpleActionServer(ACTION_NAME,
PutDownAtAction,
execute_cb=self.put_down_at,
auto_start=False)
def initialize(self):
rospy.loginfo(
'%s: waiting for audio_dump/start_audio_recording service' %
ACTION_NAME)
rospy.wait_for_service('audio_dump/start_audio_recording')
rospy.loginfo(
'%s: connected to audio_dump/start_audio_recording service' %
ACTION_NAME)
rospy.loginfo(
'%s: waiting for audio_dump/stop_audio_recording service' %
ACTION_NAME)
rospy.wait_for_service('audio_dump/stop_audio_recording')
rospy.loginfo(
'%s: connected to audio_dump/stop_audio_recording service' %
ACTION_NAME)
rospy.loginfo('%s: waiting for GraspPlanning service' % ACTION_NAME)
rospy.wait_for_service('/GraspPlanning')
rospy.loginfo('%s: connected to GraspPlanning service' % ACTION_NAME)
rospy.loginfo(
'%s: waiting for wubble2_left_arm_kinematics/get_ik_solver_info service'
% ACTION_NAME)
rospy.wait_for_service(
'/wubble2_left_arm_kinematics/get_ik_solver_info')
rospy.loginfo(
'%s: connected to wubble2_left_arm_kinematics/get_ik_solver_info service'
% ACTION_NAME)
rospy.loginfo(
'%s: waiting for wubble2_left_arm_kinematics/get_ik service' %
ACTION_NAME)
rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik')
rospy.loginfo(
'%s: connected to wubble2_left_arm_kinematics/get_ik service' %
ACTION_NAME)
rospy.loginfo(
'%s: waiting for environment_server/set_planning_scene_diff service'
% ACTION_NAME)
rospy.wait_for_service('/environment_server/set_planning_scene_diff')
rospy.loginfo('%s: connected to set_planning_scene_diff service' %
ACTION_NAME)
rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' %
ACTION_NAME)
self.posture_controller.wait_for_server()
rospy.loginfo('%s: connected to wubble_gripper_grasp_action' %
ACTION_NAME)
rospy.loginfo('%s: waiting for move_left_arm action server' %
ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server' %
ACTION_NAME)
rospy.loginfo('%s: waiting for wubble_grasp_status service' %
ACTION_NAME)
rospy.wait_for_service('/wubble_grasp_status')
rospy.loginfo('%s: connected to wubble_grasp_status service' %
ACTION_NAME)
rospy.loginfo('%s: waiting for find_cluster_bounding_box service' %
ACTION_NAME)
rospy.wait_for_service('/find_cluster_bounding_box')
rospy.loginfo('%s: connected to find_cluster_bounding_box service' %
ACTION_NAME)
self.action_server.start()
def find_ik_for_grasping_pose(self, pose_stamped):
solver_info = self.get_solver_info_srv()
arm_joints = solver_info.kinematic_solver_info.joint_names
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = GRIPPER_LINK_FRAME
req.ik_request.pose_stamped = pose_stamped
try:
current_state = rospy.wait_for_message('/joint_states', JointState,
2.0)
req.ik_request.ik_seed_state.joint_state.name = arm_joints
req.ik_request.ik_seed_state.joint_state.position = [
current_state.position[current_state.name.index(j)]
for j in arm_joints
]
if not self.set_planning_scene_diff_client():
rospy.logerr(
'%s: Find IK for Grasp: failed to set planning scene diff'
% ACTION_NAME)
return None
ik_result = self.get_ik_srv(req)
if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS:
return ik_result.solution
else:
rospy.logerr(
'%s: failed to find an IK for requested grasping pose, aborting'
% ACTION_NAME)
return None
except:
rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME)
return None
def find_grasp_pose(self,
target,
collision_object_name='',
collision_support_surface_name=''):
"""
target = GraspableObject
collision_object_name = name of target in collision map
collision_support_surface_name = name of surface target is touching
"""
req = GraspPlanningRequest()
req.arm_name = ARM_GROUP_NAME
req.target = target
req.collision_object_name = collision_object_name
req.collision_support_surface_name = collision_support_surface_name
rospy.loginfo(
'%s: trying to find a good grasp for graspable object %s' %
(ACTION_NAME, collision_object_name))
grasping_result = self.grasp_planning_srv(req)
if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS:
rospy.logerr('%s: unable to find a feasable grasp, aborting' %
ACTION_NAME)
return None
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = grasping_result.grasps[
0].grasp_posture.header.frame_id
pose_stamped.pose = grasping_result.grasps[0].grasp_pose
rospy.loginfo('%s: found good grasp, looking for corresponding IK' %
ACTION_NAME)
return self.find_ik_for_grasping_pose(pose_stamped)
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
def close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
rospy.sleep(1)
grasp_status = self.get_grasp_status_srv()
return grasp_status.is_hand_occupied
def find_cluster_bounding_box_center(self, cluster):
fcbb = FindClusterBoundingBoxRequest()
fcbb.cluster = cluster
res = self.find_cluster_bbox(fcbb)
if res.error_code == FindClusterBoundingBoxResponse.TF_ERROR:
rospy.logerr(
'Unable to find bounding box for requested point cluster')
return None
return res.pose.pose.position
def put_down_at(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
bbox_center = self.find_cluster_bounding_box_center(
goal.graspable_object.cluster)
if not bbox_center: self.action_server.set_aborted()
goal.target_location.z = bbox_center.z
x_dist = goal.target_location.x - bbox_center.x
y_dist = goal.target_location.y - bbox_center.y
target = goal.graspable_object
target.cluster.points = [
Point32(p.x + x_dist, p.y + y_dist, p.z)
for p in target.cluster.points
]
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
ik_solution = self.find_grasp_pose(target, collision_object_name,
collision_support_surface_name)
if ik_solution:
# disable collisions between gripper and target object
ordered_collision_operations = OrderedCollisionOperations()
collision_operation = CollisionOperation()
collision_operation.object1 = collision_object_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.01
# disable collisions between arm and table
collision_operation3 = CollisionOperation()
collision_operation3.object1 = collision_support_surface_name
collision_operation3.object2 = ARM_GROUP_NAME
collision_operation3.operation = CollisionOperation.DISABLE
collision_operation3.penetration_distance = 0.01
ordered_collision_operations.collision_operations = [
collision_operation, collision_operation2, collision_operation3
]
# set gripper padding to 0
gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS])
# move into pre-grasp pose
if not move_arm_joint_goal(
self.move_arm_client,
ik_solution.joint_state.name,
ik_solution.joint_state.position,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
return
# record put down sound with 0.5 second padding before and after
self.start_audio_recording_srv(InfomaxAction.GRASP,
goal.category_id)
rospy.sleep(0.5)
self.open_gripper()
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
self.action_server.set_succeeded(
PutDownAtResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted put down failed' % ACTION_NAME)
self.action_server.set_aborted()
class HokuyoLaserActionServer():
def __init__(self):
# Initialize constants
self.error_threshold = 0.0175 # Report success if error reaches below threshold
self.signal = 1
# Initialize new node
rospy.init_node(NAME, anonymous=True)
controller_name = rospy.get_param('~controller')
# Initialize publisher & subscriber for tilt
self.laser_tilt = JointControllerState()
self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64)
self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal)
self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True)
rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state)
rospy.wait_for_message(controller_name + '/state', JointControllerState)
# Initialize tilt action server
self.result = HokuyoLaserTiltResult()
self.feedback = HokuyoLaserTiltFeedback()
self.feedback.tilt_position = self.laser_tilt.process_value
self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback)
rospy.loginfo("%s: Ready to accept goals", NAME)
def process_tilt_state(self, tilt_data):
self.laser_tilt = tilt_data
def reset_tilt_position(self, offset=0.0):
self.laser_tilt_pub.publish(offset)
rospy.sleep(0.5)
def execute_callback(self, goal):
r = rospy.Rate(100)
self.joint_speed_srv(2.0)
self.reset_tilt_position(goal.offset)
delta = goal.amplitude - goal.offset
target_speed = delta / goal.duration
timeout_threshold = rospy.Duration(5.0) + rospy.Duration.from_sec(goal.duration)
self.joint_speed_srv(target_speed)
print "delta = %f, target_speed = %f" % (delta, target_speed)
self.result.success = True
self.result.tilt_position = self.laser_tilt.process_value
rospy.loginfo("%s: Executing laser tilt %s time(s)", NAME, goal.tilt_cycles)
# Tilt laser goal.tilt_cycles amount of times.
for i in range(1, goal.tilt_cycles + 1):
self.feedback.progress = i
# Issue 2 commands for each cycle
for j in range(2):
if j % 2 == 0:
target_tilt = goal.offset + goal.amplitude # Upper tilt limit
self.signal = 0
else:
target_tilt = goal.offset # Lower tilt limit
self.signal = 1
# Publish target command to controller
self.laser_tilt_pub.publish(target_tilt)
start_time = rospy.Time.now()
current_time = start_time
while abs(target_tilt - self.laser_tilt.process_value) > self.error_threshold:
#delta = abs(target_tilt - self.laser_tilt.process_value)
#time_left = goal.duration - (rospy.Time.now() - start_time).to_sec()
#target_speed = delta / time_left
#self.joint_speed_srv(target_speed)
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
return
# Publish current head position as feedback
self.feedback.tilt_position = self.laser_tilt.process_value
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > timeout_threshold):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
return
r.sleep()
signal = LaserScannerSignal()
signal.header.stamp = current_time
signal.signal = self.signal
self.laser_signal_pub.publish(signal)
#rospy.sleep(0.5)
if self.result.success:
rospy.loginfo("%s: Goal Completed", NAME)
self.result.tilt_position = self.laser_tilt.process_value
self.server.set_succeeded(self.result)
class PushObjectActionServer:
def __init__(self):
self.start_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/start_audio_recording', StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/stop_audio_recording', StopAudioRecording)
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status',
GraspStatus)
self.interpolated_ik_params_srv = rospy.ServiceProxy(
'/l_interpolated_ik_motion_plan_set_params',
SetInterpolatedIKMotionPlanParams)
self.interpolated_ik_srv = rospy.ServiceProxy(
'/l_interpolated_ik_motion_plan', GetMotionPlan)
self.set_planning_scene_diff_client = rospy.ServiceProxy(
'/environment_server/set_planning_scene_diff',
SetPlanningSceneDiff)
self.get_fk_srv = rospy.ServiceProxy(
'/wubble2_left_arm_kinematics/get_fk', GetPositionFK)
self.trajectory_filter_srv = rospy.ServiceProxy(
'/trajectory_filter_unnormalizer/filter_trajectory',
FilterJointTrajectory)
self.trajectory_controller = SimpleActionClient(
'/l_arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.attached_object_pub = rospy.Publisher(
'/attached_collision_object', AttachedCollisionObject)
self.action_server = SimpleActionServer(ACTION_NAME,
PushObjectAction,
execute_cb=self.push_object,
auto_start=False)
def initialize(self):
rospy.loginfo(
'%s: waiting for audio_dump/start_audio_recording service' %
ACTION_NAME)
rospy.wait_for_service('audio_dump/start_audio_recording')
rospy.loginfo(
'%s: connected to audio_dump/start_audio_recording service' %
ACTION_NAME)
rospy.loginfo(
'%s: waiting for audio_dump/stop_audio_recording service' %
ACTION_NAME)
rospy.wait_for_service('audio_dump/stop_audio_recording')
rospy.loginfo(
'%s: connected to audio_dump/stop_audio_recording service' %
ACTION_NAME)
rospy.loginfo(
'%s: waiting for l_interpolated_ik_motion_plan_set_params service'
% ACTION_NAME)
rospy.wait_for_service('/l_interpolated_ik_motion_plan_set_params')
rospy.loginfo(
'%s: connected to l_interpolated_ik_motion_plan_set_params service'
% ACTION_NAME)
rospy.loginfo('%s: waiting for l_interpolated_ik_motion_plan service' %
ACTION_NAME)
rospy.wait_for_service('/l_interpolated_ik_motion_plan')
rospy.loginfo(
'%s: connected to l_interpolated_ik_motion_plan service' %
ACTION_NAME)
rospy.loginfo(
'%s: waiting for environment_server/set_planning_scene_diff service'
% ACTION_NAME)
rospy.wait_for_service('/environment_server/set_planning_scene_diff')
rospy.loginfo('%s: connected to set_planning_scene_diff service' %
ACTION_NAME)
rospy.loginfo(
'%s: waiting for wubble2_left_arm_kinematics/get_fk service' %
ACTION_NAME)
rospy.wait_for_service('/wubble2_left_arm_kinematics/get_fk')
rospy.loginfo(
'%s: connected to wubble2_left_arm_kinematics/get_fk service' %
ACTION_NAME)
rospy.loginfo(
'%s: waiting for trajectory_filter_unnormalizer/filter_trajectory service'
% ACTION_NAME)
rospy.wait_for_service(
'/trajectory_filter_unnormalizer/filter_trajectory')
rospy.loginfo(
'%s: connected to trajectory_filter_unnormalizer/filter_trajectory service'
% ACTION_NAME)
rospy.loginfo(
'%s: waiting for l_arm_controller/follow_joint_trajectory' %
ACTION_NAME)
self.trajectory_controller.wait_for_server()
rospy.loginfo(
'%s: connected to l_arm_controller/follow_joint_trajectory' %
ACTION_NAME)
self.action_server.start()
def create_pose_stamped(self, pose, frame_id='base_link'):
"""
Creates a PoseStamped message from a list of 7 numbers (first three are
position and next four are orientation:
pose = [px,py,pz, ox,oy,oz,ow]
"""
m = PoseStamped()
m.header.frame_id = frame_id
m.header.stamp = rospy.Time()
m.pose.position = Point(*pose[0:3])
m.pose.orientation = Quaternion(*pose[3:7])
return m
#pretty-print list to string
def pplist(self, list_to_print):
return ' '.join(['%5.3f' % x for x in list_to_print])
def get_interpolated_ik_motion_plan(self,
start_pose,
goal_pose,
start_angles,
joint_names,
pos_spacing=0.01,
rot_spacing=0.1,
consistent_angle=math.pi / 9,
collision_aware=True,
collision_check_resolution=1,
steps_before_abort=-1,
num_steps=0,
ordered_collision_operations=None,
frame='base_footprint',
start_from_end=0,
max_joint_vels=[1.5] * 7,
max_joint_accs=[8.0] * 7):
res = self.interpolated_ik_params_srv(num_steps, consistent_angle,
collision_check_resolution,
steps_before_abort, pos_spacing,
rot_spacing, collision_aware,
start_from_end, max_joint_vels,
max_joint_accs)
req = GetMotionPlanRequest()
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_angles
req.motion_plan_request.start_state.multi_dof_joint_state.poses = [
start_pose.pose
]
req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [
GRIPPER_LINK_FRAME
]
req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [
start_pose.header.frame_id
]
pos_constraint = PositionConstraint()
pos_constraint.position = goal_pose.pose.position
pos_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.position_constraints = [
pos_constraint,
]
orient_constraint = OrientationConstraint()
orient_constraint.orientation = goal_pose.pose.orientation
orient_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.orientation_constraints = [
orient_constraint,
]
#req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
#req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
#if ordered_collision_operations is not None:
# req.motion_plan_request.ordered_collision_operations = ordered_collision_operations
res = self.interpolated_ik_srv(req)
return res
def check_cartesian_path_lists(self,
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
pos_spacing=0.03,
rot_spacing=0.1,
consistent_angle=math.pi / 7.0,
collision_aware=True,
collision_check_resolution=1,
steps_before_abort=1,
num_steps=0,
ordered_collision_operations=None,
frame='base_link'):
start_pose = self.create_pose_stamped(approachpos + approachquat,
frame)
goal_pose = self.create_pose_stamped(grasppos + graspquat, frame)
return self.get_interpolated_ik_motion_plan(
start_pose, goal_pose, start_angles, ARM_JOINTS, pos_spacing,
rot_spacing, consistent_angle, collision_aware,
collision_check_resolution, steps_before_abort, num_steps,
ordered_collision_operations, frame)
def push_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
current_state = rospy.wait_for_message('l_arm_controller/state',
FollowJointTrajectoryFeedback)
start_angles = current_state.actual.positions
full_state = rospy.wait_for_message('/joint_states', JointState)
req = GetPositionFKRequest()
req.header.frame_id = 'base_link'
req.fk_link_names = [GRIPPER_LINK_FRAME]
req.robot_state.joint_state = full_state
if not self.set_planning_scene_diff_client():
rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME)
self.action_server.set_aborted()
return
pose = self.get_fk_srv(req).pose_stamped[0].pose
frame_id = 'base_link'
approachpos = [pose.position.x, pose.position.y, pose.position.z]
approachquat = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
push_distance = 0.40
grasppos = [
pose.position.x, pose.position.y - push_distance, pose.position.z
]
graspquat = [0.00000, 0.00000, 0.70711, -0.70711]
# attach object to gripper, they will move together
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
# disable collisions between attached object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.02
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [
collision_operation1, collision_operation2
]
res = self.check_cartesian_path_lists(
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
frame=frame_id,
ordered_collision_operations=ordered_collision_operations)
error_code_dict = {
ArmNavigationErrorCodes.SUCCESS: 0,
ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
ArmNavigationErrorCodes.PLANNING_FAILED: 4
}
trajectory_len = len(res.trajectory.joint_trajectory.points)
trajectory = [
res.trajectory.joint_trajectory.points[i].positions
for i in range(trajectory_len)
]
vels = [
res.trajectory.joint_trajectory.points[i].velocities
for i in range(trajectory_len)
]
times = [
res.trajectory.joint_trajectory.points[i].time_from_start
for i in range(trajectory_len)
]
error_codes = [
error_code_dict[error_code.val]
for error_code in res.trajectory_error_codes
]
# if even one code is not 0, abort
if sum(error_codes) != 0:
for ind in range(len(trajectory)):
rospy.loginfo("error code " + str(error_codes[ind]) +
" pos : " + self.pplist(trajectory[ind]))
rospy.loginfo("")
for ind in range(len(trajectory)):
rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() +
"vels: " + self.pplist(vels[ind]))
rospy.logerr('%s: attempted push failed' % ACTION_NAME)
self.action_server.set_aborted()
return
req = FilterJointTrajectoryRequest()
req.trajectory = res.trajectory.joint_trajectory
req.trajectory.points = req.trajectory.points[
1:] # skip zero velocity point
req.allowed_time = rospy.Duration(2.0)
filt_res = self.trajectory_filter_srv(req)
goal = FollowJointTrajectoryGoal()
goal.trajectory = filt_res.trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
rospy.sleep(0.5)
self.trajectory_controller.send_goal(goal)
self.trajectory_controller.wait_for_result()
state = self.trajectory_controller.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(
PushObjectResult(sound_msg.recorded_sound))
return
rospy.logerr('%s: attempted push failed' % ACTION_NAME)
self.stop_audio_recording_srv(False)
self.action_server.set_aborted()
class axServer:
def __init__(self, name):
self.fullname = name
self.jointPositions = []
self.jointVelocities = []
self.jointAccelerations = []
self.jointNamesFromConfig = []
for configJoint in configJoints:
self.jointNamesFromConfig.append(configJoint['name'])
self.jointPositions.append(0.0)
self.jointVelocities.append(0.0)
self.jointAccelerations.append(0.0)
if 'mimic_joints' in configJoint:
for mimic in configJoint['mimic_joints']:
self.jointNamesFromConfig.append(mimic['name'])
self.jointPositions.append(0.0)
self.jointVelocities.append(0.0)
self.jointAccelerations.append(0.0)
self.pointsQueue = []
self.lastTimeFromStart = 0.0
startPositions = [0.0, -0.9, -0.9, 0.0, 0.0, 0.0]
startVelocities = [0.5]*6
self.jointPositions[1] = -0.9
dynamixelChain.move_angles_sync(ids[:6], startPositions, startVelocities)
# self.joint_state_pub = rospy.Publisher('/joint_states', JointState)
self.server = SimpleActionServer(self.fullname,
FollowJointTrajectoryAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def execute_cb(self, goal):
startTime = rospy.Time.now().to_sec()
rospy.loginfo(goal)
jNames = goal.trajectory.joint_names
self.pointsQueue = goal.trajectory.points
for a in range(len(self.pointsQueue)):
while rospy.Time.now().to_sec() - startTime < self.pointsQueue[a].time_from_start.to_sec():
rospy.sleep(0.01)
print 'sleeping...'
rospy.loginfo(rospy.Time.now().to_sec() - startTime)
if rospy.Time.now().to_sec() - startTime - self.pointsQueue[a].time_from_start.to_sec() < 0.05:
# self.executePoint(point, jNames)
if a + 1 < len(self.pointsQueue):
self.executePoint(self.pointsQueue[a], self.pointsQueue[a + 1], jNames)
else:
self.executePoint(self.pointsQueue[a], self.pointsQueue[a], jNames)
# self.publishJointStates()
self.lastTimeFromStart = rospy.Duration.from_sec(0.0)
self.pointsQueue = []
self.server.set_succeeded()
# def executePoint(self, point, jNames):
def executePoint(self, point, nextPoint, jNames):
# rospy.loginfo(point)
startTime = datetime.datetime.now()
print 'point time from start is: ' + str(point.time_from_start.to_sec())
for x in range(len(jNames)):
for y in range(len(self.jointPositions)):
if jNames[x] == self.jointNamesFromConfig[y]:
self.jointPositions[y] = point.positions[x]
self.jointVelocities[y] = point.velocities[x]
self.jointAccelerations[y] = point.accelerations[x]
# jointMover(y, point.positions[x],
# nonZeroVelocity(point.velocities[x]))
poss = matchServoVals(jNames, point.positions)
poss = matchServoVals(jNames, nextPoint.positions)
print poss
vels = nonZeroVelocities(matchServoVals(jNames, point.velocities))
print vels
accs = matchServoVals(jNames, point.accelerations)
print accs
dynamixelChain.move_angles_sync(ids[:6], poss, vels)
endTime = datetime.datetime.now()
print 'executePoint took: ' + str(endTime - startTime)
def publishJointStates(self):
# rospy.loginfo(self.jointPositions)
jState = JointState()
jState.header.stamp = rospy.Time.now()
jState.name = self.jointNamesFromConfig
jState.position = self.jointPositions
jState.velocity = self.jointVelocities
jState.effort = self.jointAccelerations
self.joint_state_pub.publish(jState)
class PushObjectActionServer:
def __init__(self):
self.start_audio_recording_srv = rospy.ServiceProxy("/audio_dump/start_audio_recording", StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy("/audio_dump/stop_audio_recording", StopAudioRecording)
self.get_grasp_status_srv = rospy.ServiceProxy("/wubble_grasp_status", GraspStatus)
self.interpolated_ik_params_srv = rospy.ServiceProxy(
"/l_interpolated_ik_motion_plan_set_params", SetInterpolatedIKMotionPlanParams
)
self.interpolated_ik_srv = rospy.ServiceProxy("/l_interpolated_ik_motion_plan", GetMotionPlan)
self.set_planning_scene_diff_client = rospy.ServiceProxy(
"/environment_server/set_planning_scene_diff", SetPlanningSceneDiff
)
self.get_fk_srv = rospy.ServiceProxy("/wubble2_left_arm_kinematics/get_fk", GetPositionFK)
self.trajectory_filter_srv = rospy.ServiceProxy(
"/trajectory_filter_unnormalizer/filter_trajectory", FilterJointTrajectory
)
self.trajectory_controller = SimpleActionClient(
"/l_arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction
)
self.attached_object_pub = rospy.Publisher("/attached_collision_object", AttachedCollisionObject)
self.action_server = SimpleActionServer(
ACTION_NAME, PushObjectAction, execute_cb=self.push_object, auto_start=False
)
def initialize(self):
rospy.loginfo("%s: waiting for audio_dump/start_audio_recording service" % ACTION_NAME)
rospy.wait_for_service("audio_dump/start_audio_recording")
rospy.loginfo("%s: connected to audio_dump/start_audio_recording service" % ACTION_NAME)
rospy.loginfo("%s: waiting for audio_dump/stop_audio_recording service" % ACTION_NAME)
rospy.wait_for_service("audio_dump/stop_audio_recording")
rospy.loginfo("%s: connected to audio_dump/stop_audio_recording service" % ACTION_NAME)
rospy.loginfo("%s: waiting for l_interpolated_ik_motion_plan_set_params service" % ACTION_NAME)
rospy.wait_for_service("/l_interpolated_ik_motion_plan_set_params")
rospy.loginfo("%s: connected to l_interpolated_ik_motion_plan_set_params service" % ACTION_NAME)
rospy.loginfo("%s: waiting for l_interpolated_ik_motion_plan service" % ACTION_NAME)
rospy.wait_for_service("/l_interpolated_ik_motion_plan")
rospy.loginfo("%s: connected to l_interpolated_ik_motion_plan service" % ACTION_NAME)
rospy.loginfo("%s: waiting for environment_server/set_planning_scene_diff service" % ACTION_NAME)
rospy.wait_for_service("/environment_server/set_planning_scene_diff")
rospy.loginfo("%s: connected to set_planning_scene_diff service" % ACTION_NAME)
rospy.loginfo("%s: waiting for wubble2_left_arm_kinematics/get_fk service" % ACTION_NAME)
rospy.wait_for_service("/wubble2_left_arm_kinematics/get_fk")
rospy.loginfo("%s: connected to wubble2_left_arm_kinematics/get_fk service" % ACTION_NAME)
rospy.loginfo("%s: waiting for trajectory_filter_unnormalizer/filter_trajectory service" % ACTION_NAME)
rospy.wait_for_service("/trajectory_filter_unnormalizer/filter_trajectory")
rospy.loginfo("%s: connected to trajectory_filter_unnormalizer/filter_trajectory service" % ACTION_NAME)
rospy.loginfo("%s: waiting for l_arm_controller/follow_joint_trajectory" % ACTION_NAME)
self.trajectory_controller.wait_for_server()
rospy.loginfo("%s: connected to l_arm_controller/follow_joint_trajectory" % ACTION_NAME)
self.action_server.start()
def create_pose_stamped(self, pose, frame_id="base_link"):
"""
Creates a PoseStamped message from a list of 7 numbers (first three are
position and next four are orientation:
pose = [px,py,pz, ox,oy,oz,ow]
"""
m = PoseStamped()
m.header.frame_id = frame_id
m.header.stamp = rospy.Time()
m.pose.position = Point(*pose[0:3])
m.pose.orientation = Quaternion(*pose[3:7])
return m
# pretty-print list to string
def pplist(self, list_to_print):
return " ".join(["%5.3f" % x for x in list_to_print])
def get_interpolated_ik_motion_plan(
self,
start_pose,
goal_pose,
start_angles,
joint_names,
pos_spacing=0.01,
rot_spacing=0.1,
consistent_angle=math.pi / 9,
collision_aware=True,
collision_check_resolution=1,
steps_before_abort=-1,
num_steps=0,
ordered_collision_operations=None,
frame="base_footprint",
start_from_end=0,
max_joint_vels=[1.5] * 7,
max_joint_accs=[8.0] * 7,
):
res = self.interpolated_ik_params_srv(
num_steps,
consistent_angle,
collision_check_resolution,
steps_before_abort,
pos_spacing,
rot_spacing,
collision_aware,
start_from_end,
max_joint_vels,
max_joint_accs,
)
req = GetMotionPlanRequest()
req.motion_plan_request.start_state.joint_state.name = joint_names
req.motion_plan_request.start_state.joint_state.position = start_angles
req.motion_plan_request.start_state.multi_dof_joint_state.poses = [start_pose.pose]
req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [GRIPPER_LINK_FRAME]
req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [start_pose.header.frame_id]
pos_constraint = PositionConstraint()
pos_constraint.position = goal_pose.pose.position
pos_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint]
orient_constraint = OrientationConstraint()
orient_constraint.orientation = goal_pose.pose.orientation
orient_constraint.header.frame_id = goal_pose.header.frame_id
req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint]
# req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS]
# req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS])
# if ordered_collision_operations is not None:
# req.motion_plan_request.ordered_collision_operations = ordered_collision_operations
res = self.interpolated_ik_srv(req)
return res
def check_cartesian_path_lists(
self,
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
pos_spacing=0.03,
rot_spacing=0.1,
consistent_angle=math.pi / 7.0,
collision_aware=True,
collision_check_resolution=1,
steps_before_abort=1,
num_steps=0,
ordered_collision_operations=None,
frame="base_link",
):
start_pose = self.create_pose_stamped(approachpos + approachquat, frame)
goal_pose = self.create_pose_stamped(grasppos + graspquat, frame)
return self.get_interpolated_ik_motion_plan(
start_pose,
goal_pose,
start_angles,
ARM_JOINTS,
pos_spacing,
rot_spacing,
consistent_angle,
collision_aware,
collision_check_resolution,
steps_before_abort,
num_steps,
ordered_collision_operations,
frame,
)
def push_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo("%s: preempted" % ACTION_NAME)
self.action_server.set_preempted()
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
start_angles = current_state.actual.positions
full_state = rospy.wait_for_message("/joint_states", JointState)
req = GetPositionFKRequest()
req.header.frame_id = "base_link"
req.fk_link_names = [GRIPPER_LINK_FRAME]
req.robot_state.joint_state = full_state
if not self.set_planning_scene_diff_client():
rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME)
self.action_server.set_aborted()
return
pose = self.get_fk_srv(req).pose_stamped[0].pose
frame_id = "base_link"
approachpos = [pose.position.x, pose.position.y, pose.position.z]
approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
push_distance = 0.40
grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z]
graspquat = [0.00000, 0.00000, 0.70711, -0.70711]
# attach object to gripper, they will move together
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
# disable collisions between attached object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.02
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
res = self.check_cartesian_path_lists(
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
frame=frame_id,
ordered_collision_operations=ordered_collision_operations,
)
error_code_dict = {
ArmNavigationErrorCodes.SUCCESS: 0,
ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
ArmNavigationErrorCodes.PLANNING_FAILED: 4,
}
trajectory_len = len(res.trajectory.joint_trajectory.points)
trajectory = [res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len)]
vels = [res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len)]
times = [res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len)]
error_codes = [error_code_dict[error_code.val] for error_code in res.trajectory_error_codes]
# if even one code is not 0, abort
if sum(error_codes) != 0:
for ind in range(len(trajectory)):
rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
rospy.loginfo("")
for ind in range(len(trajectory)):
rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind]))
rospy.logerr("%s: attempted push failed" % ACTION_NAME)
self.action_server.set_aborted()
return
req = FilterJointTrajectoryRequest()
req.trajectory = res.trajectory.joint_trajectory
req.trajectory.points = req.trajectory.points[1:] # skip zero velocity point
req.allowed_time = rospy.Duration(2.0)
filt_res = self.trajectory_filter_srv(req)
goal = FollowJointTrajectoryGoal()
goal.trajectory = filt_res.trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
rospy.sleep(0.5)
self.trajectory_controller.send_goal(goal)
self.trajectory_controller.wait_for_result()
state = self.trajectory_controller.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(PushObjectResult(sound_msg.recorded_sound))
return
rospy.logerr("%s: attempted push failed" % ACTION_NAME)
self.stop_audio_recording_srv(False)
self.action_server.set_aborted()
class AbstractHMIServer(object):
"""
Abstract base class for a hmi client
>>> class HMIServer(AbstractHMIServer):
... def __init__(self):
... pass
... def _determine_answer(self, description, spec, choices):
... return QueryResult()
... def _set_succeeded(self, result):
... print result
>>> server = HMIServer()
>>> from hmi_msgs.msg import QueryGoal
>>> goal = QueryGoal(description='q', spec='spec', choices=[])
>>> server._execute_cb(goal)
raw_result: ''
results: []
>>> class HMIServer(AbstractHMIServer):
... def __init__(self):
... pass
>>> server = HMIServer()
Traceback (most recent call last):
...
TypeError: Can't instantiate abstract class HMIServer with abstract methods _determine_answer
"""
__metaclass__ = ABCMeta
def __init__(self, name):
self._action_name = name
self._server = SimpleActionServer(name, QueryAction,
execute_cb=self._execute_cb, auto_start=False)
self._server.start()
rospy.loginfo('HMI server started on "%s"', name)
def _execute_cb(self, goal):
# TODO: refactor this somewhere
choices = {}
for choice in goal.choices:
if choice.id in choices:
rospy.logwarn('duplicate key "%s" in answer', choice.id)
else:
choices[choice.id] = choice.values
rospy.loginfo('I got a question: %s', goal.description)
rospy.loginfo('This is the spec: %s, %s', trim_string(goal.spec), repr(choices))
try:
result = self._determine_answer(description=goal.description,
spec=goal.spec,
choices=choices,
is_preempt_requested=self._server.is_preempt_requested)
except Exception as e:
# rospy.logwarn('_determine_answer raised an exception: %s', e)
# import pdb; pdb.set_trace()
tb = traceback.format_exc()
rospy.logerr('_determine_answer raised an exception: %s' % tb)
self._server.set_aborted()
else:
# we've got a result or a cancel
if result:
self._set_succeeded(result=result.to_ros(self._action_name))
rospy.loginfo('result: %s', result)
else:
rospy.loginfo('cancelled')
self._server.set_aborted(text="Cancelled by user")
def _set_succeeded(self, result):
self._server.set_succeeded(result)
def _publish_feedback(self):
self._server.publish_feedback(QueryActionFeedback())
@abstractmethod
def _determine_answer(self, description, spec, choices, is_preempt_requested):
'''
Overwrite this method to provide custom implementations
Return the answer
Return None if nothing is heared
Raise an Exception if an error occured
'''
pass
class PathExecutor:
goal_index = 0
reached_all_nodes = True
def __init__(self):
rospy.loginfo('__init__ start')
# create a node
rospy.init_node(NODE)
# Action server to receive goals
self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction,
self.handle_path, auto_start=False)
self.path_server.start()
# publishers & clients
self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path)
# get parameters from launch file
self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
# action server based on use_obstacle_avoidance
if self.use_obstacle_avoidance == False:
self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
else:
self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)
self.goal_client.wait_for_server()
# other fields
self.goal_index = 0
self.executePathGoal = None
self.executePathResult = ExecutePathResult()
def handle_path(self, paramExecutePathGoal):
'''
Action server callback to handle following path in succession
'''
rospy.loginfo('handle_path')
self.goal_index = 0
self.executePathGoal = paramExecutePathGoal
self.executePathResult = ExecutePathResult()
if self.executePathGoal is not None:
self.visualization_publisher.publish(self.executePathGoal.path)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
if not self.path_server.is_active():
return
if self.path_server.is_preempt_requested():
rospy.loginfo('Preempt requested...')
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_preempted()
return
if self.goal_index < len(self.executePathGoal.path.poses):
moveto_goal = MoveToGoal()
moveto_goal.target_pose = self.executePathGoal.path.poses[self.goal_index]
self.goal_client.send_goal(moveto_goal, done_cb=self.handle_goal,
feedback_cb=self.handle_goal_preempt)
# Wait for result
while self.goal_client.get_result() is None:
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
else:
rospy.loginfo('Done processing goals')
self.goal_client.cancel_goal()
self.path_server.set_succeeded(self.executePathResult, 'Done processing goals')
return
rate.sleep()
self.path_server.set_aborted(self.executePathResult, 'Aborted. The node has been killed.')
def handle_goal(self, state, result):
'''
Handle goal events (succeeded, preempted, aborted, unreachable, ...)
Send feedback message
'''
feedback = ExecutePathFeedback()
feedback.pose = self.executePathGoal.path.poses[self.goal_index]
# state is GoalStatus message as shown here:
# http://docs.ros.org/diamondback/api/actionlib_msgs/html/msg/GoalStatus.html
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Succeeded finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(True)
feedback.reached = True
else:
rospy.loginfo("Failed finding goal %d", self.goal_index + 1)
self.executePathResult.visited.append(False)
feedback.reached = False
if not self.executePathGoal.skip_unreachable:
rospy.loginfo('Failed finding goal %d, not skipping...', self.goal_index + 1)
# Stop bug2
self.goal_client.cancel_goal()
# Stop path_server
self.path_server.set_succeeded(self.executePathResult,
'Unreachable goal in path. Done processing goals.')
#self.path_server.set_preempted()
#return
self.path_server.publish_feedback(feedback)
self.goal_index = self.goal_index + 1
def handle_goal_preempt(self, state):
'''
Callback for goal_client to check for preemption from path_server
'''
if self.path_server.is_preempt_requested():
self.goal_client.cancel_goal()
class PickUpActionServer():
def __init__(self):
# Initialize new node
rospy.init_node(NAME)#, anonymous=False)
#initialize the clients to interface with
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
self.move_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
# Initialize tf listener (remove?)
self.tf = tf.TransformListener()
# Initialize erratic base action server
self.result = SmartArmGraspResult()
self.feedback = SmartArmGraspFeedback()
self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback)
#define the offsets
# These offsets were determines expirmentally using the simulator
# They were tested using points stamped with /map
self.xOffset = 0.025
self.yOffset = 0.0
self.zOffset = 0.12 #.05 # this does work!
rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME)
rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )
def move_to(self, frame_id, position, orientation, vicinity=0.0):
goal = ErraticBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = frame_id
goal.target_pose.pose.position = position
goal.target_pose.pose.orientation = orientation
goal.vicinity_range = vicinity
self.move_client.send_goal(goal)
#print "going into loop"
while (not self.move_client.wait_for_result(rospy.Duration(0.01))):
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.move_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.move_client.get_state()
#(almost) blatent copy / past from wubble_head_action.py. Err, it's going to the wrong position
def transform_target_point(self, point, frameId):
#rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z)
# Wait for tf info (time-out in 5 seconds)
self.tf.waitForTransform(frameId, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
# Transform target point & retrieve the pan angle
return self.tf.transformPoint(frameId, point)
#UNUSED
def move_base_feedback_cb(self, fb):
self.feedback.base_position = fb.base_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
# This moves the arm into a positions based on angles (in rads)
# It depends on the sources code in wubble_actions
def move_arm(self, shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate):
goal = SmartArmGoal()
goal.target_joints = [shoulder_pan, shoulder_tilt, elbow_tilt, wrist_rotate]
self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
self.arm_client.wait_for_goal_to_finish()
while not self.arm_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.arm_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.arm_client.get_state()
# This moves the wrist of the arm to the x, y, z coordinates
def reach_at(self, frame_id, x, y, z):
goal = SmartArmGoal()
goal.target_point = PointStamped()
goal.target_point.header.frame_id = frame_id
goal.target_point.point.x = x
goal.target_point.point.y = y
goal.target_point.point.z = z
rospy.loginfo("%s: Original point is '%s' [%f, %f, %f]", NAME, frame_id,\
goal.target_point.point.x, goal.target_point.point.y, goal.target_point.point.z)
goal.target_point = self.transform_target_point(goal.target_point, '/arm_base_link');
rospy.loginfo("%s: Transformed point is '/armbaselink' [%f, %f, %f]", NAME, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
goal.target_point.point.x = goal.target_point.point.x + self.xOffset
goal.target_point.point.y = goal.target_point.point.y + self.yOffset
goal.target_point.point.z = goal.target_point.point.z + self.zOffset
rospy.loginfo("%s: Transformed and Offset point is '/armbaselink' [%f, %f, %f]", NAME, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
self.arm_client.send_goal(goal, None, None, self.arm_position_feedback_cb)
self.arm_client.wait_for_goal_to_finish()
while not self.arm_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.arm_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.arm_client.get_state()
#This method is used passes updates from arm positions actions to the feedback
#of this server
def arm_position_feedback_cb(self, fb):
self.feedback.arm_position = fb.arm_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
#This moves the gripper to the given angles
def move_gripper(self, left_finger, right_finger):
goal = SmartArmGripperGoal()
goal.target_joints = [left_finger, right_finger]
self.gripper_client.send_goal(goal, None, None, self.gripper_position_feedback_cb)
#gripper_client.wait_for_goal_to_finish()
while not self.gripper_client.wait_for_result(rospy.Duration(0.01)) :
# check for preemption
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.gripper_client.cancel_goal()
return GoalStatus.PREEMPTED
return self.gripper_client.get_state()
#This method is used passes updates from arm positions actions to the feedback
#of this server
def gripper_position_feedback_cb(self, fb):
self.feedback.gripper_position = fb.gripper_position
if self.server.is_active():
self.server.publish_feedback(self.feedback)
#This method sets the results of the goal to the last feedback values
def set_results(self, success):
self.result.success = success
self.result.arm_position = self.feedback.arm_position
self.result.gripper_position = self.feedback.gripper_position
self.server.set_succeeded(self.result)
#This is the callback function that is executed whenever the server
#recieves a request
def execute_callback(self, goal):
rospy.loginfo("%s: Executing Grasp Action [%s, %f, %f, %f]", NAME, \
goal.target_point.header.frame_id, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
rospy.loginfo( "%s: Moving the arm to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(0.0, 1.972222, -1.972222, 0.0)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 1st Move Arm (to cobra pose) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 1st Move Arm (to cobra pose) Failed", NAME)
self.set_results(False)
return
position = Point(x = goal.target_point.point.x, y = goal.target_point.point.y)
orientation = Quaternion(w=1.0)
self.move_to('/map', position, orientation, 0.5)
self.move_to('/map', position, orientation, 0.2)
rospy.sleep(0.2)
rospy.loginfo( "%s: Opening gripper", NAME)
#open the gripper
result = self.move_gripper(0.2, -0.2)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Open Gripper Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: Open Gripper Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Moving the arm to the object", NAME)
#move the arm to the correct posisions
result = self.reach_at(goal.target_point.header.frame_id, \
goal.target_point.point.x, \
goal.target_point.point.y, \
goal.target_point.point.z)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 2nd Move Arm (to object) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 2nd Move Arm (to object) Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Moving the elbow joint to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(self.feedback.arm_position[0], self.feedback.arm_position[1], \
-3.14 / 2 - self.feedback.arm_position[1], self.feedback.arm_position[3])
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Moving the elbow joint Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: Moving the elbow joint Failed", NAME)
self.set_results(False)
return
rospy.loginfo( "%s: Closing gripper", NAME)
#close the gripper
result = self.move_gripper(-1.5, 1.5)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: Close Gripper Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
#this actions 'succeeds' if it times out
rospy.loginfo( "%s: Moving the arm to the cobra pose", NAME)
#move the arm into the cobra position
result = self.move_arm(0.0, 1.972222, -1.972222, 0.0)
if result == GoalStatus.PREEMPTED: #action has failed
rospy.loginfo("%s: 3rd Move Arm (to cobra pose) Preempted", NAME)
self.server.set_preempted()
self.set_results(False)
return
elif result != GoalStatus.SUCCEEDED:
rospy.loginfo("%s: 3rd Move Arm (to cobra pose) Failed", NAME)
self.set_results(False)
return
#action has succeeded
rospy.loginfo("%s: Grasp Action Succeed [%s, %f, %f, %f]", NAME, \
goal.target_point.header.frame_id, goal.target_point.point.x, \
goal.target_point.point.y, goal.target_point.point.z)
self.set_results(True)
"""
class ObjectDetectionActionServer(object):
_detector_handler = None # type: SingleImageDetectionHandler
_cloud_topic = None # type: str
_filtered_cloud_pub = None # type: rospy.Publisher
_tf_listener = None # type: tf.TransformListener
_target_frame = None # type: str
_cv_bridge = None # type: CvBridge
def __init__(self, action_name, timeout_s=10., **kwargs):
rospy.loginfo('broadcasting action server: ' + action_name)
self._action_server = SimpleActionServer(action_name,
DetectObjectsAction,
execute_cb=self._execute_cb,
auto_start=False)
self._timeout_s = timeout_s
self._initialize(**kwargs)
self._action_server.start()
def _initialize(self, **kwargs):
detection_class = kwargs.get('detection_class', None)
if not issubclass(detection_class, ImageDetectorBase):
raise ValueError(
'"detection_class" is not of ImageDetectorBase type')
class_annotation_file = kwargs.get('class_annotation_file', None)
if not class_annotation_file or not os.path.exists(
class_annotation_file):
raise ValueError('invalid value for "class_annotation_file": ' +
class_annotation_file)
kwargs_file = kwargs.get('kwargs_file', None)
if not kwargs_file or not os.path.exists(kwargs_file):
raise ValueError('invalid value for "kwargs_file": ' + kwargs_file)
# image detection
self._detector_handler = SingleImageDetectionHandler(
detection_class, class_annotation_file, kwargs_file,
'/mas_perception/detection_result')
self._cloud_topic = kwargs.get('cloud_topic', None)
if not self._cloud_topic:
raise ValueError('no cloud topic specified')
self._filtered_cloud_pub = rospy.Publisher('filtered_cloud',
PointCloud2,
queue_size=1)
self._tf_listener = tf.TransformListener()
self._target_frame = kwargs.get('target_frame', '/base_link')
rospy.loginfo('will transform all poses to frame: ' +
self._target_frame)
def _execute_cb(self, _):
# subscribe and wait for cloud message
rospy.loginfo('Object detection action request received')
rospy.loginfo('Waiting for cloud message...')
try:
cloud_msg = rospy.wait_for_message(self._cloud_topic,
PointCloud2,
timeout=self._timeout_s)
except rospy.ROSInterruptException:
# shutdown event interrupts waiting for point cloud
raise
except rospy.ROSException:
rospy.logwarn(
'timeout waiting for cloud topic "{0}" after "{1}" seconds'.
format(self._cloud_topic, self._timeout_s))
self._action_server.set_aborted()
return
rospy.loginfo('Cloud message received; detecting objects...')
img_msg = cloud_msg_to_image_msg(cloud_msg)
original_img_msg = copy.deepcopy(img_msg)
try:
bounding_boxes, classes, confidences = self._detector_handler.process_image_msg(
img_msg)
except RuntimeError as e:
self._action_server.set_aborted(text=e.message)
return
rospy.loginfo('transforming cloud to frame: ' + self._target_frame)
try:
transformed_cloud_msg = transform_cloud_with_listener(
cloud_msg, self._target_frame, self._tf_listener)
except RuntimeError as e:
self._action_server.set_aborted(text=e.message)
return
if self._filtered_cloud_pub.get_num_connections() > 0:
self._filtered_cloud_pub.publish(filtered_cloud)
rospy.loginfo('creating action result and setting success')
result = ObjectDetectionActionServer._get_action_result(
transformed_cloud_msg, bounding_boxes, classes, confidences)
result.image = original_img_msg
self._action_server.set_succeeded(result)
@staticmethod
def _get_action_result(cloud_msg, bounding_boxes, classes, confidences):
"""
:type cloud_msg: PointCloud2
:type bounding_boxes: list
:type classes: list
:type confidences: list
:rtype: DetectObjectsResult
"""
result = DetectObjectsResult()
for index, box in enumerate(bounding_boxes):
detected_obj = get_obj_msg_from_detection(
cloud_msg, box, classes[index], confidences[index],
cloud_msg.header.frame_id)
box_msg = detected_obj.bounding_box
rospy.loginfo(
"Adding object '%s', position (%.3f, %.3f, %.3f), dimensions (%.3f, %.3f, %.3f)"
% (classes[index], box_msg.center.x, box_msg.center.y,
box_msg.center.z, box_msg.dimensions.x,
box_msg.dimensions.y, box_msg.dimensions.z))
result.objects.objects.append(detected_obj)
return result
class axServer:
def __init__(self, name, configJoints):
self.fullname = name
self.jointNames = []
for configJoint in configJoints:
self.jointNames.append(configJoint["name"])
self.jointNames = self.jointNames[:4]
self.failureState = False
self.goalPositions = [0.0, -0.9, 0.0, 0.0]
self.goalVelocities = [0.5] * 4
self.move_chain()
# todo: add method for checking arm's actual positions
self.actualPositions = [0.0, -0.9, 0.0, 0.0]
self.server = SimpleActionServer(
self.fullname, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False
)
self.server.start()
def move_chain(self):
# moves the Dynamixel Chain to the object's goalPositions
# and goalVelocities
orderedIDs = [[1], [2, 3], [4, 5], [6]]
commandIDs = []
commandPositions = []
commandVelocities = []
for i in range(len(self.goalPositions)):
for orderedID in orderedIDs[i]:
commandIDs.append(orderedID)
commandPositions.append(self.goalPositions[i])
commandVelocities.append(self.goalVelocities[i])
dynamixelChain.move_angles_sync(commandIDs, commandPositions, commandVelocities)
def execute_cb(self, goal):
startTime = rospy.Time.now().to_sec()
rospy.loginfo(goal)
jNames = goal.trajectory.joint_names
pointsQueue = goal.trajectory.points
for a in range(len(pointsQueue)):
pointTime = pointsQueue[a].time_from_start.to_sec()
timeSlept = 0.0
while rospy.Time.now().to_sec() - startTime < pointTime:
rospy.sleep(0.01)
timeSlept += 0.01
print "slept " + str(timeSlept) + " seconds between points"
rospy.loginfo(rospy.Time.now().to_sec() - startTime)
if a == 0:
self.executePoint(pointsQueue[a], pointsQueue[a], jNames)
else:
self.executePoint(pointsQueue[a - 1], pointsQueue[a], jNames)
self.server.set_succeeded()
def executePoint(self, point1, point2, jNames):
# moves arm to point2's positions at the greater
# absolute Velocites of the two points
startTime = datetime.datetime.now()
for x in range(len(jNames)):
if jNames[x] in self.jointNames:
y = self.jointNames.index(jNames[x])
self.goalPositions[y] = point2.positions[x]
if abs(point1.velocities[x]) > abs(point2.velocities[x]):
self.goalVelocities[y] = abs(point1.velocities[x])
else:
self.goalVelocities[y] = abs(point2.velocities[x])
else:
print "we have a problem: joint names are not lining up"
self.move_chain()
endTime = datetime.datetime.now()
print "executePoint took: " + str(endTime - startTime)
def checkFailureState(self):
if self.failureState:
print "I am currently in a failure state."
class LiftObjectActionServer:
def __init__(self):
self.get_grasp_status_srv = rospy.ServiceProxy("/wubble_grasp_status", GraspStatus)
self.start_audio_recording_srv = rospy.ServiceProxy("/audio_dump/start_audio_recording", StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy("/audio_dump/stop_audio_recording", StopAudioRecording)
self.posture_controller = SimpleActionClient("/wubble_gripper_grasp_action", GraspHandPostureExecutionAction)
self.move_arm_client = SimpleActionClient("/move_left_arm", MoveArmAction)
self.attached_object_pub = rospy.Publisher("/attached_collision_object", AttachedCollisionObject)
self.action_server = SimpleActionServer(
ACTION_NAME, LiftObjectAction, execute_cb=self.lift_object, auto_start=False
)
def initialize(self):
rospy.loginfo("%s: waiting for wubble_grasp_status service" % ACTION_NAME)
rospy.wait_for_service("/wubble_grasp_status")
rospy.loginfo("%s: connected to wubble_grasp_status service" % ACTION_NAME)
rospy.loginfo("%s: waiting for wubble_gripper_grasp_action" % ACTION_NAME)
self.posture_controller.wait_for_server()
rospy.loginfo("%s: connected to wubble_gripper_grasp_action" % ACTION_NAME)
rospy.loginfo("%s: waiting for audio_dump/start_audio_recording service" % ACTION_NAME)
rospy.wait_for_service("audio_dump/start_audio_recording")
rospy.loginfo("%s: connected to audio_dump/start_audio_recording service" % ACTION_NAME)
rospy.loginfo("%s: waiting for audio_dump/stop_audio_recording service" % ACTION_NAME)
rospy.wait_for_service("audio_dump/stop_audio_recording")
rospy.loginfo("%s: connected to audio_dump/stop_audio_recording service" % ACTION_NAME)
rospy.loginfo("%s: waiting for move_left_arm action server" % ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo("%s: connected to move_left_arm action server" % ACTION_NAME)
self.action_server.start()
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
def lift_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo("%s: preempted" % ACTION_NAME)
self.action_server.set_preempted()
collision_support_surface_name = goal.collision_support_surface_name
# disable collisions between grasped object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = GRIPPER_GROUP_NAME
collision_operation2.object2 = collision_support_surface_name
collision_operation2.operation = CollisionOperation.DISABLE
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
# this is a hack to make arm lift an object faster
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.REMOVE
obj.object.id = collision_support_surface_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
joint_names = current_state.joint_names
joint_positions = current_state.actual.positions
start_angles = [joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS]
start_angles[0] = start_angles[0] - 0.3 # move shoulder up a bit
if not move_arm_joint_goal(
self.move_arm_client,
ARM_JOINTS,
start_angles,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations,
):
self.action_server.set_aborted()
return
self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id)
if move_arm_joint_goal(
self.move_arm_client,
ARM_JOINTS,
LIFT_POSITION,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations,
):
# check if are still holding an object after lift is done
grasp_status = self.get_grasp_status_srv()
# if the object is still in hand after lift is done we are good
if grasp_status.is_hand_occupied:
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(LiftObjectResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr("%s: attempted lift failed" % ACTION_NAME)
self.action_server.set_aborted()
return
class OnlineBagger(object):
BAG_TOPIC = '/online_bagger/bag'
def __init__(self):
"""
Make dictionary of dequeues.
Subscribe to set of topics defined by the yaml file in directory
Stream topics up to a given stream time, dump oldest messages when limit is reached
Set up service to bag n seconds of data default to all of available data
"""
self.successful_subscription_count = 0 # successful subscriptions
self.iteration_count = 0 # number of iterations
self.streaming = True
self.get_params()
if len(self.subscriber_list) == 0:
rospy.logwarn('No topics selected to subscribe to. Closing.')
rospy.signal_shutdown('No topics to subscribe to')
return
self.make_dicts()
self._action_server = SimpleActionServer(OnlineBagger.BAG_TOPIC, BagOnlineAction,
execute_cb=self.start_bagging, auto_start=False)
self.subscribe_loop()
rospy.loginfo('Remaining Failed Topics: {}\n'.format(
self.get_subscriber_list(False)))
self._action_server.start()
def get_subscriber_list(self, status):
"""
Get string of all topics, if their subscribe status matches the input (True / False)
Outputs each topics: time_buffer(float in seconds), subscribe_statue(bool), topic(string)
"""
sub_list = ''
for topic in self.subscriber_list.keys():
if self.subscriber_list[topic][1] == status:
sub_list = sub_list + \
'\n{:13}, {}'.format(self.subscriber_list[topic], topic)
return sub_list
def get_params(self):
"""
Retrieve parameters from param server.
"""
self.dir = rospy.get_param('~bag_package_path', default=None)
# Handle bag directory for MIL bag script
if self.dir is None and 'BAG_DIR' in os.environ:
self.dir = os.environ['BAG_DIR']
self.stream_time = rospy.get_param(
'~stream_time', default=30) # seconds
self.resubscribe_period = rospy.get_param(
'~resubscribe_period', default=3.0) # seconds
self.dated_folder = rospy.get_param(
'~dated_folder', default=True) # bool
self.subscriber_list = {}
topics_param = rospy.get_param('~topics', default=[])
# Add topics from rosparam to subscribe list
for topic in topics_param:
time = topic[1] if len(topic) == 2 else self.stream_time
self.subscriber_list[topic[0]] = (time, False)
def add_unique_topic(topic):
if topic not in self.subscriber_list:
self.subscriber_list[topic] = (self.stream_time, False)
def add_env_var(var):
for topic in var.split():
add_unique_topic(topic)
# Add topics from MIL bag script environment variables
if 'BAG_ALWAYS' in os.environ:
add_env_var(os.environ['BAG_ALWAYS'])
for key in os.environ.keys():
if key[0:4] == 'bag_':
add_env_var(os.environ[key])
rospy.loginfo(
'Default stream_time: {} seconds'.format(self.stream_time))
rospy.loginfo('Bag Directory: {}'.format(self.dir))
def make_dicts(self):
"""
Make dictionary with sliceable deques() that will be filled with messages and time stamps.
Subscriber list contains all of the topics, their stream time and their subscription status:
A True status for a given topic corresponds to a successful subscription
A False status indicates a failed subscription.
Stream time for an individual topic is specified in seconds.
For Example:
self.subscriber_list[0:1] = [['/odom', 300 ,False], ['/absodom', 300, True]]
Indicates that '/odom' has not been subscribed to, but '/absodom' has been subscribed to
self.topic_messages is a dictionary of deques containing a list of tuples.
Dictionary Keys contain topic names
Each value for each topic contains a deque
Each deque contains a list of tuples
Each tuple contains a message and its associated time stamp
For example:
'/odom' is a potential topic name
self.topic_message['/odom'] is a deque
self.topic_message['/odom'][0] is the oldest message available in the deque
and its time stamp if available. It is a tuple with each element: (time_stamp, msg)
self.topic_message['/odom'][0][0] is the time stamp for the oldest message
self.topic_message['/odom'][0][1] is the message associated with the oldest topic
"""
self.topic_messages = {}
class SliceableDeque(deque):
def __getitem__(self, index):
if isinstance(index, slice):
return type(self)(itertools.islice(self, index.start,
index.stop, index.step))
return deque.__getitem__(self, index)
for topic in self.subscriber_list:
self.topic_messages[topic] = SliceableDeque(deque())
rospy.loginfo('Initial subscriber_list: {}'.format(
self.get_subscriber_list(False)))
def subscribe_loop(self):
"""
Continue to subscribe until at least one topic is successful,
then break out of loop and be called in the callback function to prevent the function
from locking up.
"""
self.resubscriber = None
i = 0
# if self.successful_subscription_count == 0 and not
# rospy.is_shutdown():
while self.successful_subscription_count == 0 and not rospy.is_shutdown():
self.subscribe()
rospy.sleep(0.1)
i = i + 1
if i % 1000 == 0:
rospy.logdebug('still subscribing!')
rospy.loginfo("Subscribed to {} of {} topics, will try again every {} seconds".format(
self.successful_subscription_count, len(self.subscriber_list), self.resubscribe_period))
self.resubscriber = rospy.Timer(rospy.Duration(
self.resubscribe_period), self.subscribe)
def subscribe(self, time_info=None):
"""
Subscribe to the topics defined in the yaml configuration file
Function checks subscription status True/False of each topic
if True: topic has already been sucessfully subscribed to
if False: topic still needs to be subscribed to and
subscriber will be run.
Each element in self.subscriber list is a list [topic, Bool]
where the Bool tells the current status of the subscriber (sucess/failure).
Return number of topics that failed subscription
"""
if self.successful_subscription_count == len(self.subscriber_list):
if self.resubscriber is not None:
self.resubscriber.shutdown()
rospy.loginfo(
'All topics subscribed too! Shutting down resubscriber')
for topic, (time, subscribed) in self.subscriber_list.items():
if not subscribed:
msg_class = rostopic.get_topic_class(topic)
if msg_class[1] is not None:
self.successful_subscription_count += 1
rospy.Subscriber(topic, msg_class[0],
lambda msg, _topic=topic: self.bagger_callback(msg, _topic))
self.subscriber_list[topic] = (time, True)
def get_topic_duration(self, topic):
"""
Return current time duration of topic
"""
return self.topic_messages[topic][-1][0] - self.topic_messages[topic][0][0]
def get_header_time(self, msg):
"""
Retrieve header time if available
"""
if hasattr(msg, 'header'):
return msg.header.stamp
else:
return rospy.get_rostime()
def get_time_index(self, topic, requested_seconds):
"""
Return the index for the time index for a topic at 'n' seconds from the end of the dequeue
For example, to bag the last 10 seconds of data, the index for 10 seconds back from the most
recent message can be obtained with this function.
The number of requested seconds should be the number of seoncds desired from
the end of deque. (ie. requested_seconds = 10 )
If the desired time length of the bag is greater than the available messages it will output a
message and return how ever many seconds of data are avaiable at the moment.
Seconds is of a number type (not a rospy.Time type) (ie. int, float)
"""
topic_duration = self.get_topic_duration(topic).to_sec()
if topic_duration == 0:
return 0
ratio = requested_seconds / topic_duration
index = int(self.get_topic_message_count(topic) * (1 - min(ratio, 1)))
return index
def bagger_callback(self, msg, topic):
"""
Streaming callback function, stops streaming during bagging process
also pops off msgs from dequeue if stream size is greater than specified stream_time
Stream, callback function does nothing if streaming is not active
"""
if not self.streaming:
return
self.iteration_count = self.iteration_count + 1
time = self.get_header_time(msg)
self.topic_messages[topic].append((time, msg))
time_diff = self.get_topic_duration(topic)
# verify streaming is popping off and recording topics
if self.iteration_count % 100 == 0:
rospy.logdebug("{} has {} messages spanning {} seconds".format(
topic, self.get_topic_message_count(topic), round(time_diff.to_sec(), 2)))
while time_diff > rospy.Duration(self.subscriber_list[topic][0]) and not rospy.is_shutdown():
self.topic_messages[topic].popleft()
time_diff = self.get_topic_duration(topic)
def get_topic_message_count(self, topic):
"""
Return number of messages available in a topic
"""
return len(self.topic_messages[topic])
def get_total_message_count(self):
"""
Returns total number of messages across all topics
"""
total_message_count = 0
for topic in self.topic_messages.keys():
total_message_count = total_message_count + \
self.get_topic_message_count(topic)
return total_message_count
def _get_default_filename(self):
return str(datetime.date.today()) + '-' + str(datetime.datetime.now().time())[0:8]
def get_bag_name(self, filename=''):
"""
Create ros bag save directory
If no bag name is provided, the current date/time is used as default.
"""
# If directory param is not set, default to $HOME/bags/<date>
default_dir = self.dir
if default_dir is None:
default_dir = os.path.join(os.environ['HOME'], 'bags')
# if dated folder param is set to True, append current date to
# directory
if self.dated_folder is True:
default_dir = os.path.join(default_dir, str(datetime.date.today()))
# Split filename from directory
bag_dir, bag_name = os.path.split(filename)
bag_dir = os.path.join(default_dir, bag_dir)
if not os.path.exists(bag_dir):
os.makedirs(bag_dir)
# Create default filename if only directory specified
if bag_name == '':
bag_name = self._get_default_filename()
# Make sure filename ends in .bag, add otherwise
if bag_name[-4:] != '.bag':
bag_name = bag_name + '.bag'
return os.path.join(bag_dir, bag_name)
def start_bagging(self, req):
"""
Dump all data in dictionary to bags, temporarily stops streaming
during the bagging process, resumes streaming when over.
If bagging is already false because of an active call to this service
"""
result = BagOnlineResult()
if self.streaming is False:
result.status = 'Bag Request came in while bagging, priority given to prior request'
result.success = False
self._action_server.set_aborted(result)
return
bag = None
try:
self.streaming = False
result.filename = self.get_bag_name(req.bag_name)
bag = rosbag.Bag(result.filename, 'w')
requested_seconds = req.bag_time
selected_topics = req.topics.split()
feedback = BagOnlineFeedback()
total_messages = 0
bag_topics = {}
for topic, (time, subscribed) in self.subscriber_list.iteritems():
if not subscribed:
continue
# Exclude topics that aren't in topics service argument
# If topics argument is empty string, include all topics
if len(selected_topics) > 0 and topic not in selected_topics:
continue
if len(self.topic_messages[topic]) == 0:
continue
if req.bag_time == 0:
index = 0
else:
index = self.get_time_index(topic, requested_seconds)
total_messages += len(self.topic_messages[topic][index:])
bag_topics[topic] = index
if total_messages == 0:
result.success = False
result.status = 'no messages'
self._action_server.set_aborted(result)
self.streaming = True
bag.close()
return
self._action_server.publish_feedback(feedback)
msg_inc = 0
for topic, index in bag_topics.iteritems():
for msgs in self.topic_messages[topic][index:]:
bag.write(topic, msgs[1], t=msgs[0])
if msg_inc % 50 == 0: # send feedback every 50 messages
feedback.progress = float(msg_inc) / total_messages
self._action_server.publish_feedback(feedback)
msg_inc += 1
# empty deque when done writing to bag
self.topic_messages[topic].clear()
feedback.progress = 1.0
self._action_server.publish_feedback(feedback)
bag.close()
except Exception as e:
result.success = False
result.status = 'Exception while writing bag: ' + str(e)
self._action_server.set_aborted(result)
self.streaming = True
if bag is not None:
bag.close()
return
rospy.loginfo('Bag written to {}'.format(result.filename))
result.success = True
self._action_server.set_succeeded(result)
self.streaming = True
class ConstructStaticCollisionMapServer:
def __init__(self,
node_name = 'construct_static_collision_map_server',
action_name = 'construct_static_collision_map_action',
head_topic = '/head_traj_controller/point_head_action',
static_collision_map_topic = '/make_static_collision_map'
):
rospy.init_node(node_name)
rospy.loginfo ( 'waiting for SimpleActionClient: %s'%( head_topic ) )
self.head_client = SimpleActionClient(head_topic,PointHeadAction )
self.head_client.wait_for_server( )
self.take_static_collision_map_client = SimpleActionClient(static_collision_map_topic, MakeStaticCollisionMapAction)
self._action_server = SimpleActionServer(action_name, ConstructStaticCollisionMapAction, execute_cb=self.execute_cb)
rospy.loginfo('ready')
def execute_cb(self, goal):
self.scan_table()
goal = MakeStaticCollisionMapGoal()
self.take_static_collision_map_client.send_goal(goal)
self._action_server.set_succeeded()
def move_head(self, x, y, z,
min_dur = 0.0,
max_velocity = 1.0,
frame_id = 'base_link',
timeout = 5.0):
point = PointStamped()
point.header.frame_id = frame_id
point.header.stamp = rospy.Time.now()
point.point.x, point.point.y, point.point.z = x, y, z
goal = PointHeadGoal()
goal.pointing_frame = 'head_plate_frame'
goal.max_velocity = max_velocity
goal.min_duration = rospy.Duration.from_sec( min_dur )
goal.target = point
self.head_client.send_goal( goal )
self.head_client.wait_for_result( timeout =
rospy.Duration.from_sec( timeout ) )
if not self.head_client.get_state() == GoalStatus.SUCCEEDED:
rospy.logerr( 'can not move head to:\n %s'%( goal ) )
return False
return True
def scan_table(self,
x = 0.5,
y_min = -1.0, y_max = 1.0,
z = 0.7,
steps = 10,
min_dur = 1.0,
max_wait_time = 10.0
):
rospy.loginfo( 'scanning table with x = %5.3f, y_min = %5.3f, y_max = %5.3f, z = %5.3f, steps = %d'%( x, y_min, y_max, z, steps ) )
Y = np.linspace( y_min, y_max, steps )
for y in Y:
if not self.move_head( x, y, z, min_dur ):
return
rospy.loginfo( 'done scanning table!' )
class Kill():
REFRESH_RATE = 20
def __init__(self):
self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675]
self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644]
self.v = [0] * len(self.r1)
self.r_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']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self._action_name = 'kill'
self._tf = tf.TransformListener()
#initialize base controller
topic_name = '/base_controller/command'
self._base_publisher = rospy.Publisher(topic_name, Twist)
#Initialize the sound controller
self._sound_client = SoundClient()
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.pose = None
self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback)
#initialize this client
self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def marker_callback(self, marker):
if (self.pose is not None):
self.pose = marker.pose
def run(self, goal):
rospy.loginfo('Begin kill')
self.pose = goal.pose
#pose = self._tf.transformPose('/base_link', goal.pose)
self._sound_client.say('Halt!')
# turn to face the marker before opening arms (code repeated later)
r = rospy.Rate(self.REFRESH_RATE)
while(True):
pose = self.pose
if abs(pose.pose.position.y) > .1:
num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / ( 3 * abs(pose.pose.position.y)))
for i in range(num_move_y):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.y = 0
else:
break
# open arms
traj_goal_r = JointTrajectoryGoal()
traj_goal_r.trajectory.joint_names = self.r_joint_names
traj_goal_l = JointTrajectoryGoal()
traj_goal_l.trajectory.joint_names = self.l_joint_names
traj_goal_r.trajectory.points.append(JointTrajectoryPoint(positions=self.r1, velocities = self.v, time_from_start = rospy.Duration(2)))
self.r_traj_action_client.send_goal_and_wait(traj_goal_r, rospy.Duration(3))
traj_goal_l.trajectory.points.append(JointTrajectoryPoint(positions=self.l1, velocities = self.v, time_from_start = rospy.Duration(2)))
self.l_traj_action_client.send_goal_and_wait(traj_goal_l, rospy.Duration(3))
traj_goal_r.trajectory.points[0].positions = self.r2
self.r_traj_action_client.send_goal(traj_goal_r)
traj_goal_l.trajectory.points[0].positions = self.l2
self.l_traj_action_client.send_goal(traj_goal_l)
self._sound_client.say('You have the right to remain silent.')
# Keep a local copy because it will update
#pose = self.pose
#num_move_x = int((pose.pose.position.x - 0.3) * 10 / .1) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
#twist_msg = Twist()
#twist_msg.linear = Vector3(.1, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, 0.0)
#for i in range(num_move_x):
# self._base_publisher.publish(twist_msg)
# r.sleep()
# track the marker as much as we can
while True:
pose = self.pose
# too far away
if abs(pose.pose.position.x > 1.5):
rospy.loginfo('Target out of range: ' + str(pose.pose.position.x))
self._as.set_aborted()
return;
# too far to the side -> rotate
elif abs(pose.pose.position.y) > .1:
num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (3 * abs(pose.pose.position.y)))
for i in range(num_move_y):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.y = 0
#twist_msg = Twist()
#twist_msg.linear = Vector3(0.0, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (5 * abs(pose.pose.position.y)))
#self._base_publisher.publish(twist_msg)
# now we are going to move in for the kill, but only until .5 meters away (don't want to run suspect over)
elif pose.pose.position.x > .5:
num_move_x = int((pose.pose.position.x - 0.3) * self.REFRESH_RATE / .1) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(.1, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
for i in range(num_move_x):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.x = 0
#twist_msg = Twist()
#twist_msg.linear = Vector3(.1, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, 0.0)
#self._base_publisher.publish(twist_msg)
# susupect is within hugging range!!!
else:
break
r.sleep()
# after exiting the loop, we should be ready to capture -> send close arms goal
self._sound_client.say("Anything you say do can and will be used against you in a court of law.")
self.l_traj_action_client.wait_for_result(rospy.Duration(3))
self.r_traj_action_client.wait_for_result(rospy.Duration(3))
traj_goal_r.trajectory.points[0].positions = self.r3
self.r_traj_action_client.send_goal(traj_goal_r)
traj_goal_l.trajectory.points[0].positions = self.l3
self.l_traj_action_client.send_goal(traj_goal_l)
self.l_traj_action_client.wait_for_result(rospy.Duration(3))
self.r_traj_action_client.wait_for_result(rospy.Duration(3))
rospy.loginfo('End kill')
rospy.sleep(20)
self._as.set_succeeded()