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 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 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 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 MockExplorer():
def __init__(self, exploration_topic):
self.robot_pose_ = PoseStamped()
self.listener = tf.TransformListener()
self.navigation_succedes = True
self.reply = False
self.preempted = 0
self.entered_exploration = False
self.do_exploration_as_ = SimpleActionServer(
exploration_topic,
DoExplorationAction,
execute_cb=self.do_exploration_cb,
auto_start=False)
self.do_exploration_as_.start()
def __del__(self):
self.do_exploration_as_.__del__()
def do_exploration_cb(self, goal):
rospy.loginfo('do_exploration_cb')
self.entered_exploration = 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 = DoExplorationFeedback()
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.do_exploration_as_.publish_feedback(feedback)
if self.do_exploration_as_.is_preempt_requested():
self.preempted += 1
rospy.loginfo("Preempted!")
self.entered_exploration = False
self.do_exploration_as_.set_preempted(DoExplorationResult())
return None
else:
result = DoExplorationResult()
self.reply = False
self.preempted = 0
self.entered_exploration = False
if self.navigation_succedes:
self.do_exploration_as_.set_succeded(result)
else:
self.do_exploration_as_.set_aborted(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 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 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 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 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 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 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 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 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 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 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 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 CModelActionController(object):
def __init__(self, activate=True):
self._ns = rospy.get_namespace()
# Read configuration parameters
self._fb_rate = read_parameter(
self._ns + 'gripper_action_controller/publish_rate', 60.0)
self._min_gap = read_parameter(
self._ns + 'gripper_action_controller/min_gap', 0.0)
self._min_gap_counts = read_parameter(
self._ns + 'gripper_action_controller/min_gap_counts', 230.)
self._max_gap = read_parameter(
self._ns + 'gripper_action_controller/max_gap', 0.085)
self._min_speed = read_parameter(
self._ns + 'gripper_action_controller/min_speed', 0.013)
self._max_speed = read_parameter(
self._ns + 'gripper_action_controller/max_speed', 0.1)
self._min_force = read_parameter(
self._ns + 'gripper_action_controller/min_force', 40.0)
self._max_force = read_parameter(
self._ns + 'gripper_action_controller/max_force', 100.0)
# Configure and start the action server
self._status = CModelStatus()
self._name = self._ns + 'gripper_action_controller'
self._server = SimpleActionServer(self._name,
CModelCommandAction,
execute_cb=self._execute_cb,
auto_start=False)
self.js_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
rospy.Subscriber('status', CModelStatus, self._status_cb, queue_size=1)
self._cmd_pub = rospy.Publisher('command', CModelCommand, queue_size=1)
working = True
if activate and not self._ready():
rospy.sleep(2.0)
working = self._activate()
if not working:
return
self._server.start()
rospy.logdebug('%s: Started' % self._name)
def _preempt(self):
self._stop()
rospy.loginfo('%s: Preempted' % self._name)
self._server.set_preempted()
def _status_cb(self, msg):
self._status = msg
# Publish the joint_states for the gripper
js_msg = JointState()
js_msg.header.stamp = rospy.Time.now()
js_msg.name.append('robotiq_85_left_knuckle_joint')
js_msg.position.append(0.8 * self._status.gPO / self._min_gap_counts)
self.js_pub.publish(js_msg)
def _execute_cb(self, goal):
success = True
# Check that the gripper is active. If not, activate it.
if not self._ready():
if not self._activate():
rospy.logwarn(
'%s could not accept goal because the gripper is not yet active'
% self._name)
return
# check that preempt has not been requested by the client
if self._server.is_preempt_requested():
self._preempt()
return
# Clip the goal
position = np.clip(goal.position, self._min_gap, self._max_gap)
velocity = np.clip(goal.velocity, self._min_speed, self._max_speed)
force = np.clip(goal.force, self._min_force, self._max_force)
# Send the goal to the gripper and feedback to the action client
rate = rospy.Rate(self._fb_rate)
rospy.logdebug('%s: Moving gripper to position: %.3f ' %
(self._name, position))
feedback = CModelCommandFeedback()
while not self._reached_goal(position):
self._goto_position(position, velocity, force)
if rospy.is_shutdown() or self._server.is_preempt_requested():
self._preempt()
return
feedback.position = self._get_position()
feedback.stalled = self._stalled()
feedback.reached_goal = self._reached_goal(position)
self._server.publish_feedback(feedback)
rate.sleep()
if self._stalled():
break
rospy.logdebug('%s: Succeeded' % self._name)
result = CModelCommandResult()
result.position = self._get_position()
result.stalled = self._stalled()
result.reached_goal = self._reached_goal(position)
self._server.set_succeeded(result)
def _activate(self, timeout=5.0):
command = CModelCommand()
command.rACT = 1
command.rGTO = 1
command.rSP = 255
command.rFR = 150
start_time = rospy.get_time()
while not self._ready():
if rospy.is_shutdown():
self._preempt()
return False
if rospy.get_time() - start_time > timeout:
rospy.logwarn('Failed to activated gripper in ns [%s]' %
(self._ns))
return False
self._cmd_pub.publish(command)
rospy.sleep(0.1)
rospy.loginfo('Successfully activated gripper in ns [%s]' % (self._ns))
return True
def _get_position(self):
gPO = self._status.gPO
pos = np.clip((self._max_gap - self._min_gap) /
(-self._min_gap_counts) * (gPO - self._min_gap_counts),
self._min_gap, self._max_gap)
return pos
def _goto_position(self, pos, vel, force):
"""
Goto position with desired force and velocity
@type pos: float
@param pos: Gripper width in meters
@type vel: float
@param vel: Gripper speed in m/s
@type force: float
@param force: Gripper force in N
"""
command = CModelCommand()
command.rACT = 1
command.rGTO = 1
command.rPR = int(
np.clip((-self._min_gap_counts) / (self._max_gap - self._min_gap) *
(pos - self._min_gap) + self._min_gap_counts, 0,
self._min_gap_counts))
command.rSP = int(
np.clip((255) / (self._max_speed - self._min_speed) *
(vel - self._min_speed), 0, 255))
command.rFR = int(
np.clip((255) / (self._max_force - self._min_force) *
(force - self._min_force), 0, 255))
self._cmd_pub.publish(command)
def _moving(self):
return self._status.gGTO == 1 and self._status.gOBJ == 0
def _reached_goal(self, goal, tol=0.003):
return (abs(goal - self._get_position()) < tol)
def _ready(self):
return self._status.gSTA == 3 and self._status.gACT == 1
def _stalled(self):
return self._status.gOBJ == 1 or self._status.gOBJ == 2
def _stop(self):
command = CModelCommand()
command.rACT = 1
command.rGTO = 0
self._cmd_pub.publish(command)
rospy.logdebug('Stopping gripper in ns [%s]' % (self._ns))
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 NavToNode(object):
def __init__(self):
# action client
self._ac_gp = SimpleActionClient("global_planner_node_action",
GlobalPlannerAction)
self._ac_gp.wait_for_server()
self._ac_lp = SimpleActionClient("local_planner_node_action",
LocalPlannerAction)
self._ac_lp.wait_for_server()
# action servergoal
self._as = SimpleActionServer("nav_to_node_action",
NavToAction,
self.ExecuteCB,
auto_start=False)
# define goals to send
self.global_planner_goal_ = GlobalPlannerGoal()
self.local_planner_goal_ = LocalPlannerGoal()
self.new_goal_ = False
self.new_path_ = False
self.global_planner_error_code = -1
self.local_planner_error_code = -1
# new thread running the execution loop
self.thread_ = Thread(target=self.Loop)
self.thread_.start()
self._as.start()
def ExecuteCB(self, goal):
rospy.loginfo('NAVTO: Goal received')
self.global_planner_goal_.goal = goal.navgoal
self.global_planner_error_code = -1
self.local_planner_error_code = -1
self.new_goal_ = True
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
rospy.loginfo('NAVTO: preempt requested')
self._as.set_preempted()
self.new_goal_ = False
self.new_path_ = False
self._ac_gp.cancel_all_goals()
self._ac_lp.cancel_all_goals()
return
if self.global_planner_error_code == 0:
self.new_goal_ = False
self.new_path_ = False
self._ac_lp.wait_for_result()
self._as.set_succeeded()
# rospy.loginfo('NAVTO: succeeded')
break
if self.global_planner_error_code >= 1:
self.new_goal_ = False
self.new_path_ = False
self._ac_lp.wait_for_result()
self._as.set_aborted()
# rospy.loginfo('NAVTO: failed')
break
def Loop(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
self._ac_gp.cancel_all_goals()
self._ac_lp.cancel_all_goals()
if self.new_goal_:
self._ac_gp.send_goal(self.global_planner_goal_,
self.GlobalPlannerDoneCB, None,
self.GlobalPlannerFeedbackCallback)
self.new_goal_ = False
if self.new_path_:
self._ac_lp.send_goal(self.local_planner_goal_,
self.LocalPlannerDoneCallback, None,
self.LocalPlanerFeedbackCallback)
self.new_path_ = False
try:
rate.sleep()
except:
rospy.loginfo('NAVTO: exiting')
def GlobalPlannerDoneCB(self, state, result):
rospy.loginfo("NAVTO: GP, %d, %d" % (state, result.error_code))
self.global_planner_error_code = result.error_code
def GlobalPlannerFeedbackCallback(self, data):
self.local_planner_goal_.route = data.path
self.new_path_ = True
def LocalPlannerDoneCallback(self, state, result):
rospy.loginfo("NAVTO: LP, %d, %d" % (state, result.error_code))
self.local_planner_error_code = result.error_code
def LocalPlanerFeedbackCallback(self, data):
pass
class FakeTurtle:
def __init__(self):
# define the server, aut-start is set to false so we control when to
# start it
self.server = SimpleActionServer('move_turtle_action',
MoveTurtleAction,
execute_cb=self.action_cb,
auto_start=False)
# deine feedback msg object to be used for publishing feedback
self.feedback_msg = MoveTurtleFeedback()
# deine result msg object to be used for publishing result
self.result_msg = MoveTurtleResult()
self.server.start()
print("action server started ..")
self.rate = rospy.Rate(2)
def turtle_is_up_side_down(self):
# this turtle never goes up side down
# it can always move!
# (this is a dummy function, in real application you might be checking
# your robot status, for example the battery level,
# or motor temperature, etc..)
return False
def action_cb(self, goal):
print("moving turtle to pose: ", goal)
# Write here the logic to control turtle
# this "for" loop is just a demostration, in real application, you
# would be controlling the robot, publish velocity commands, read
# current pose and do some computation..
for _ in range(20):
print('executing goal')
# during execution of the goal, publish feedback msg to the client
self.server.publish_feedback(self.feedback_msg)
# always check if the client has sent a cancel msg. This way the
# task becomes preemtable
if self.server.is_preempt_requested():
# update goal status accordingly
self.server.set_preempted()
print('goal preempted')
# exit from the action_cb method
return None
# check if we need to abort for some reason. Here we abort if
# the node has been killed (ctrl+C) or if the turtle is laying on
# it's back and cannot move anyomre ;)
if rospy.is_shutdown() or self.turtle_is_up_side_down():
# update goal status accordingly
self.server.set_aborted()
print('Turtle cannot move sir! goal aborted')
return None
self.rate.sleep()
# if execution finished, set goal status to SUCCEED and publish the
# result msg
print('done')
self.server.set_succeeded(result=self.result_msg,
text="Mission completed")
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 AbstractHMIServer(object):
"""
Abstract base class for a hmi servers
"""
__metaclass__ = ABCMeta
def __init__(self, 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):
rospy.loginfo('I got a question: %s', goal.description)
rospy.loginfo('This is the grammar: %s, %s', trim_string(goal.grammar),
goal.target)
try:
result = self._determine_answer(
description=goal.description,
example_sentences=goal.example_sentences,
grammar=goal.grammar,
target=goal.target,
is_preempt_requested=self._is_preempt_requested)
except Exception as e:
tb = traceback.format_exc()
rospy.logerr('_determine_answer raised an exception: %s' % tb)
self._set_aborted()
else:
# we've got a result or a cancel
if result:
rospy.loginfo('result: %s', result)
self._set_succeeded(result=result_to_ros(result))
else:
msg = "Cancelled by user"
rospy.loginfo(msg)
self._set_aborted(text=msg)
def _set_succeeded(self, result):
self._server.set_succeeded(result)
def _set_aborted(self, text=""):
self._server.set_aborted(text=text)
def _publish_feedback(self):
self._server.publish_feedback(QueryActionFeedback())
def _is_preempt_requested(self):
return self._server.is_preempt_requested()
@abstractmethod
def _determine_answer(self, description, grammar, target,
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 move_base_fake_node:
def __init__(self):
self.action_server = SimpleActionServer(
'move_base',
MoveBaseAction,
execute_cb=self.execute_callback,
auto_start=False
) # Simple action server will pretend to be move_base
# Movement goal state
self.goal = None # Is a goal set
self.valid_goal = False # Is this a valid goal
self.current_goal_started = False # Has the goal been started (i.e. have we told our Bug algorithm to use this point and start)
self.current_goal_complete = False # Has the Bug algorithm told us it completed
self.position = None # move_base feedback reports the current direction
## TO DO!!
# Need a service provided by this node or something for the Bug algorithm to tell us it is done
# Bug service to start and stop Bug algorithm
# Bug service to set a new goal in Bug algorithm
# rospy.wait_for_service()
# rospy.wait_for_service()
self.subscriber_odometry = rospy.Subscriber(
'odom/', Odometry, self.callback_odometry
) # We need to read the robots current point for the feedback
self.subscriber_simple_goal = rospy.Subscriber(
'/move_base_simple/goal/', PoseStamped, self.callback_simple_goal
) # Our return goal is done with /move_base_simple/goal/
self.goal_pub = rospy.Publisher(
'/move_base/goal/', MoveBaseActionGoal,
queue_size=10) # /move_base_simple/goal/ gets published here
self.action_server.start()
def execute_callback(self, move_base_goal):
self.goal = move_base_goal.target_pose.pose # Set the provided goal as the current goal
rospy.logdebug('[Move Base Fake] Execute Callback: {}'.format(
str(self.goal.position)))
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
r = rospy.Rate(1)
while not rospy.is_shutdown():
# Always start by checking if there is a new goal that preempts the current one
if self.action_server.is_preempt_requested():
## TO DO!!
# Tell Bug algorithm to stop before changing or stopping goal
if self.action_server.is_new_goal_available():
new_goal = self.action_server.accept_new_goal(
) # There is a new goal
rospy.logdebug('[Move Base Fake] New Goal: {}'.format(
str(self.goal.position)))
self.goal = new_goal.target_pose.pose # Set the provided goal as the current goal
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
else:
self.action_server.set_preempted(
) # No new goal, we've just been told to stop
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
return
# Start goal
if self.valid_goal and not self.current_goal_started:
rospy.logdebug('[Move Base Fake] Starting Goal')
## TO DO !!
# Call the Bug services/topics etc to tell Bug algorithm new target point and then to start
self.current_goal_started = True # Only start once
# Feedback ever loop just reports current location
feedback = MoveBaseFeedback()
feedback.base_position.pose.position = self.position
self.action_server.publish_feedback(feedback)
# Completed is set in a callback that you need to link to a service or subscriber
if self.current_goal_complete:
rospy.logdebug('[Move Base Fake] Finishing Goal')
## TO DO!!
# Tell Bug algorithm to stop before changing or stopping goal
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
self.action_server.set_succeeded(
MoveBaseResult(), 'Goal reached') # Send success message
return
r.sleep()
# Shutdown
rospy.logdebug('[Move Base Fake] Shutting Down')
## TO DO!!
# Tell Bug algorithm to stop before changing or stopping goal
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
# you need to connect this to something being called/published from the Bug algorithm
def callback_complete(self, success):
# TO DO!!
# Implement some kind of service or subscriber so the Bug algorithm can tell this node it is complete
self.current_goal_complete = success.data
def callback_odometry(self, odom):
self.position = odom.pose.pose.position
# Simple goals get republished to the correct topic
def callback_simple_goal(self, goal):
rospy.logdebug('[Move Base Fake] Simple Goal: {}'.format(
str(goal.pose.position)))
action_goal = MoveBaseActionGoal()
action_goal.header.stamp = rospy.Time.now()
action_goal.goal.target_pose = goal
self.goal_pub.publish(action_goal)
class PathExecutor:
_feedback = ExecutePathFeedback()
_result = ExecutePathResult()
def __init__(self, name):
# self._goal_publisher = Simple
# self.pe_client_for_bug2= SimpleActionClient('/bug2/move_to',MoveToAction)
# self.pe_client_for_bug2.wait_for_server()
print "----------------- in INIT ---------------------------------------"
rospy.loginfo("Creating an action server")
# Creating a client to perform the bug2 function
self._move_to_client = SimpleActionClient('/bug2/move_to',
MoveToAction)
# Creating an action server
self._as = SimpleActionServer('/path_executor/execute_path', ExecutePathAction, \
execute_cb=self.execute_cb, auto_start=False)
# Starting the server
self._as.start()
# Using a flag to aid preemption
self.flag = True
# Using a flag to detect the completion of 3 goals
self.success = True
def execute_cb(self, goal):
# Executed every time a goal is sent by the client
self._move_to_client.wait_for_server()
# printing the goals in the terminal
rospy.loginfo(goal.path.poses)
# Creating an instance of MoveToGoal to compose a goal message
self.move = MoveToGoal()
# Iterating through the goals and sending it to the move_to_client
for i in range(len(goal.path.poses)):
if i == 2:
self.success = True
else:
self.success = False
self._pose = goal.path.poses[i]
self.move.target_pose = self._pose
self._move_to_client.send_goal(self.move,
done_cb=self.move_to_done_cb)
self._move_to_client.wait_for_result()
# self.success = True
rospy.spin()
# setting the server's preempted() function if it's requested
while flag:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
break
def move_to_done_cb(self, state, result):
if (state == 3):
#
self._result.visited.append(True)
self._feedback.reached = True
self._feedback.pose = self._pose
elif (state == 4):
self._feedback.reached = False
#
rospy.loginfo(self._result)
self._as.publish_feedback(self._feedback)
if self.success == True:
print "All goals are considered"
self._as.set_succeeded(self._result)
class AggressiveGainBuffNode(object):
def __init__(self, name="aggressive_gain_buff_action"):
self.action_name = name
self._as = SimpleActionServer(self.action_name, AggressiveGainBuffAction, execute_cb=self.ExecuteCB, auto_start=False)
self.gain_buff_state = GainBuffState.ROUTE
self.chassis_state_ = 0
self.sub_odom = rospy.Subscriber("aggressive_buff_info", AggressiveGainBuffInfo, callback=self.OdomCB)
self.chassis_mode_client = rospy.ServiceProxy("set_chassis_mode", ChassisMode)
self.chassis_arrive_state = ChassisArriveState()
self.chassis_mode_ = 0
self._ac_navto = SimpleActionClient("nav_to_node_action", NavToAction)
rospy.loginfo('GAIN_BUFF: Connecting NavTo action server...')
ret = self._ac_navto.wait_for_server()
rospy.loginfo('GAIN_BUFF: NavTo sever connected!') if ret else rospy.logerr('error: NavTo server not started!')
self.nav_to_error_code = -1
self.search_start_time = rospy.Time(0)
self._ac_look_n_move = SimpleActionClient("look_n_move_node_action", LookAndMoveAction)
rospy.loginfo('GAIN_BUFF: Connecting Look and Move action server...')
ret = self._ac_look_n_move.wait_for_server(timeout=rospy.Duration(3))
rospy.loginfo('GAIN_BUFF: Look and Move sever connected!') if ret else rospy.logerr('error: Look and Move server not started!')
self.Look_n_move_error_code = -1
self.thread_ = Thread(target=self.Loop)
self.thread_.start()
self._as.start()
def ExecuteCB(self, goal):
print 'GAIN_BUFF:Aggressive gain buff goal recieved!'
route = goal.route_index
CHASSIS_MODE = ChassisModeForm()
self.nav_to_error_code = -1
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
print 'GAIN_BUFF:PREEMPT REQ'
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self._ac_navto.cancel_all_goals()
self._ac_look_n_move.cancel_all_goals()
self._as.set_preempted()
return
if self.gain_buff_state == GainBuffState.ROUTE:
if route == 1 or route == 2:
print 'GAIN_BUFF:Route %i received' % route
if route == 1:
self.SetChassisMode(CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_ONE)
if route == 2:
self.SetChassisMode(CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_TWO)
print 'GAIN_BUFF:Wait for chassis response...'
chassis_wait_start_time = rospy.get_time()
print 'GAIN_BUFF:chassis state is %i' % (self.chassis_state_)
while self.chassis_state_ == 0 and (rospy.get_time() - chassis_wait_start_time) < CHASSIS_MODE_WAIT_TIME:
continue
if self.chassis_state_ == 1:
print 'GAIN_BUFF:Chassis has responded!'
else:
print 'GAIN_BUFF:Chassis does not respond for the new mode! Return.'
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self._as.set_aborted()
return
chassis_run_start_time = rospy.get_time()
while not (self.chassis_state_ == self.chassis_arrive_state.SUCCEEDED or (rospy.get_time()-chassis_run_start_time > CHASSIS_RUN_WAIT_TIME) ):
if self.chassis_state_ == self.chassis_arrive_state.SUCCEEDED:
# self._as.set_succeeded()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self.gain_buff_state = GainBuffState.SEARCH
self.search_start_time = rospy.get_time()
print 'GAIN_BUFF:Chassis Arrived Buff Area!'
break
else:
print 'GAIN_BUFF:Chassis Aggressive gain buff FAILED, Time out!'
self._as.set_aborted()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
return
elif route == 3:
print 'GAIN_BUFF:Route 3 received'
self.NavToBuff()
if self.nav_to_error_code == 0:
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
# self._as.set_succeeded()
self.gain_buff_state = GainBuffState.SEARCH
self.search_start_time = rospy.get_time()
print 'GAIN_BUFF:Aggressive Gain Buff Route 3 SUCCEED!'
else:
self._as.set_aborted()
print 'No valied route'
return
elif self.gain_buff_state == GainBuffState.SEARCH:
if (rospy.get_time() - self.search_start_time) < SEARCH_BUFF_WAIT_TIME:
self.SearchBuff()
else:
print 'SEARCH BUFF: Chassis Aggressive search buff Time Out!'
self._as.set_aborted()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self.gain_buff_state = GainBuffState.IDEL
return
else:
print 'Unvaild aggressive gain buff state!'
self._as.set_aborted()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
return
def Loop(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
self._ac_look_n_move.cancel_all_goals()
self._ac_navto.cancel_all_goals()
try:
rate.sleep()
except:
rospy.loginfo('GAIN_BUFF: exiting')
# Set chassis mode to use odom navigation
def SetChassisMode(self, chassis_mode):
if self.chassis_mode_ == chassis_mode:
return
rospy.wait_for_service("set_chassis_mode", timeout=5)
print 'Set chassis mode service connected!'
chassis_mode_msg = ChassisMode()
call_status = self.chassis_mode_client.call(chassis_mode)
chassis_mode_rec_ = ChassisModeResponse()
chassis_mode_rec_ = chassis_mode_msg._response_class
if(call_status and chassis_mode_rec_):
self.chassis_mode_ = chassis_mode
print 'Set Chassis Mode to %i' % chassis_mode
else:
print 'Set gimbal mode failed!'
# return odom arrive result
def OdomCB(self, data):
self.chassis_state_ = data.state
# Call NavTo Action
def NavToBuff(self):
print 'NavTo action is actived!'
g = NavToActionGoal()
for path in BuffPath:
q = tf.transformations.quaternion_from_euler(0,0,path['yaw'])
g.goal.navgoal.pose.position.x = path['x']
g.goal.navgoal.pose.position.y = path['y']
g.goal.navgoal.pose.orientation.z = q[2]
g.goal.navgoal.pose.orientation.w = q[3]
self._ac_navto.send_goal(g.goal, done_cb=self.done_cb, feedback_cb=None)
print 'Waiting for %i point ...' % (path['id'])
self._ac_navto.wait_for_result(timeout=rospy.Duration(10))
if self.nav_to_error_code != 0:
self._ac_navto.cancel_all_goals()
self._as.set_aborted()
print 'AGGRESSIVA_GAIN_BUFF: NavTo Failed!'
return
print 'The result of %i point in BuffPath is arrived!' % path['id']
def SearchBuff(self):
print 'Look and Move action is actived!'
for goal in SearchBuffGoal:
q = tf.transformations.quaternion_from_euler(0.,0.,goal['yaw'])
g = LookAndMoveActionGoal()
g.goal.relative_pose.header.frame_id = "map"
g.goal.relative_pose.pose.position.x = goal['x']
g.goal.relative_pose.pose.position.y = goal['y']
g.goal.relative_pose.pose.orientation.z = q[2]
g.goal.relative_pose.pose.orientation.w = q[3]
self._ac_look_n_move.send_goal(g.goal)
self._ac_look_n_move.wait_for_result(timeout=rospy.Duration(10))
if self.Look_n_move_error_code > 0:
self._ac_look_n_move.cancel_all_goals()
self._as.set_aborted()
print 'AGGRESSIVA_GAIN_BUFF: Look and Move Failed!'
return
# Nav_to done result
def done_cb(self,terminal_state,result):
self.nav_to_error_code = result.error_code
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 V_search(object):
def __init__(self, height, res):
self.resolution = res # Resolution of the motion
self.motion_height = height # Height of the motion to the ground
# Create search server
self.search_server = SimpleActionServer('search_server',
ExecuteSearchAction,
self.searchCallback, False)
self.server_feedback = ExecuteSearchFeedback()
self.server_result = ExecuteSearchResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.next_point = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
## variables
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
self.odometry = None
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback)
# Start trajectory server
self.search_server.start()
def trajectory_feed(self, msg):
'''
Verifies preemption requisitions
'''
if self.search_server.is_preempt_requested():
self.trajectory_client.cancel_goal()
def searchCallback(self, search_area):
'''
Execute a search for vitims into the defined area
'''
x = search_area.x # x size of the area to explore
y = search_area.y # y size of the area to explore
start = search_area.origin # Point to start the search
self.next_point.goal.position.x = start.x # Desired x position
self.next_point.goal.position.y = start.y # Desired y position
theta = 0
# Convert desired angle
q = quaternion_from_euler(0, 0, theta, 'ryxz')
self.next_point.goal.orientation.x = q[0]
self.next_point.goal.orientation.y = q[1]
self.next_point.goal.orientation.z = q[2]
self.next_point.goal.orientation.w = q[3]
x_positions = ceil(x / self.resolution)
y_positions = ceil(y / self.resolution)
x_count = 0 # Counter of steps (in meters traveled)
direction = 1 # Direction of the motion (right, left or up)
trials = 0 # v_search trials
while not rospy.is_shutdown():
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.server_feedback.current_pose = self.odometry
self.odometry_me.release()
if self.search_server.is_preempt_requested():
self.search_server.set_preempted(self.server_result)
return
self.search_server.publish_feedback(self.server_feedback)
self.sonar_me.acquire()
# print("Current height from ground:\n\n{}".format(self.current_height)) # Current distance from ground
h_error = self.motion_height - self.current_height
self.sonar_me.release()
self.odometry_me.acquire()
self.next_point.goal.position.z = self.odometry.position.z + h_error # Desired z position
self.odometry_me.release()
self.trajectory_client.send_goal(self.next_point,
feedback_cb=self.trajectory_feed)
self.trajectory_client.wait_for_result() # Wait for the result
result = self.trajectory_client.get_state(
) # Get the state of the action
# print(result)
if result == GoalStatus.SUCCEEDED:
# Verifies if all the area have been searched
if (self.next_point.goal.position.x == (start.x + x)) and (
(self.next_point.goal.position.y == (start.y + y))):
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.odometry_me.release()
self.search_server.set_succeeded(self.server_result)
return
last_direction = direction
direction = self.square_function(
x_count, 2 * x) # Get the direction of the next step
if last_direction != direction:
# drone moves on y axis
theta = pi / 2
self.next_point.goal.position.y += y / y_positions
elif direction == 1:
# drone moves to the right
theta = 0
self.next_point.goal.position.x += x / x_positions
x_count += x / x_positions
elif direction == -1:
# drone moves to the left
theta = pi
self.next_point.goal.position.x -= x / x_positions
x_count += x / x_positions
# Convert desired angle
q = quaternion_from_euler(0, 0, theta, 'ryxz')
self.next_point.goal.orientation.x = q[0]
self.next_point.goal.orientation.y = q[1]
self.next_point.goal.orientation.z = q[2]
self.next_point.goal.orientation.w = q[3]
elif result == GoalStatus.ABORTED:
trials += 1
if trials == 2:
self.search_server.set_aborted(self.server_result)
return
def square_function(self, x, max_x):
'''
Function to simulate a square wave function
'''
if round(sin(2 * pi * x / max_x), 2) > 0:
return 1
elif round(sin(2 * pi * x / max_x), 2) < 0:
return -1
else:
if round(cos(2 * pi * x / max_x), 2) > 0:
return 1
else:
return -1
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
self.odometry_me.release()
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 FaceTracker(object):
DIST_THRESH = 0.0
def __init__(self):
self.rate = rospy.Rate(2)
self.tf = tf.TransformListener()
self.eye_speed = 0.2
self.head_speed = 0.7
self.blender_frame = "blender"
self.default_position = [1, 0, 0] # Looking straight ahead 1 metre
self.last_position = None
# Publishers for making the face and eyes look at a point
self.face_target_pub = rospy.Publisher("/blender_api/set_face_target",
Target,
queue_size=1)
self.gaze_target_pub = rospy.Publisher("/blender_api/set_gaze_target",
Target,
queue_size=1)
# Gaze action server
self.action_srv = SimpleActionServer("/gaze_action",
GazeAction,
execute_cb=self.execute_cb,
auto_start=False)
self.action_srv.start()
def execute_cb(self, goal):
rospy.loginfo("Target goal received: " + str(goal))
target_frame = goal.target
while not rospy.is_shutdown(
) and not self.action_srv.is_preempt_requested(
) and self.action_srv.is_active():
if self.tf.frameExists(target_frame) and self.tf.frameExists(
self.blender_frame):
time = self.tf.getLatestCommonTime(target_frame,
self.blender_frame)
position, quaternion = self.tf.lookupTransform(
self.blender_frame, target_frame, time)
update_target = self.last_position is None
if self.last_position is not None:
dist = FaceTracker.distance(position, self.last_position)
update_target = dist > FaceTracker.DIST_THRESH
if update_target:
self.gaze_at_point(position)
self.action_srv.publish_feedback(GazeActionFeedback())
self.rate.sleep()
# If gaze has been cancelled then set default position
self.gaze_at_point(self.default_position)
@staticmethod
def distance(p1, p2):
return math.sqrt(
math.pow(p1[0] - p2[0], 2) + math.pow(p1[1] - p2[1], 2) +
math.pow(p1[2] - p2[2], 2))
def gaze_at_point(self, position):
x = position[0]
y = position[1]
z = position[2]
self.point_eyes_at_point(x, y, z, self.eye_speed)
self.face_toward_point(x, y, z, self.head_speed)
self.last_position = position
def point_eyes_at_point(self, x, y, z, speed):
""" Turn the robot's eyes towards the given target point
:param float x: metres forward
:param float y: metres to robots left
:param float z:
:param float speed:
:return: None
"""
msg = Target()
msg.x = x
msg.y = y
msg.z = z
msg.speed = speed
self.gaze_target_pub.publish(msg)
rospy.logdebug("published gaze_at(x={}, y={}, z={}, speed={})".format(
x, y, z, speed))
def face_toward_point(self, x, y, z, speed):
""" Turn the robot's face towards the given target point.
:param float x: metres forward
:param float y: metres to robots left
:param float z:
:param float speed:
:return: None
"""
msg = Target()
msg.x = x
msg.y = y
msg.z = z
msg.speed = speed
self.face_target_pub.publish(msg)
rospy.logdebug("published face_(x={}, y={}, z={}, speed={})".format(
x, y, z, speed))
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 BakerArmServer(script):
DOF = 5
DEFAULT_POSITION = [0.6, 1., -2., 1., 0.]
TRANSPORT_POSITION = [0.9, 1., -2., 1., 0.]
IS_GRIPPER_AVAILABLE = False
def __init__(self, name, status=ArmStatus.NO_TRASHCAN):
self.verbose_ = True
self.confirm_ = False
self.name_ = name
self.status_ = status
self.joint_values_ = [0.0] * self.DOF
self.mutex_ = Lock()
(self.trashcan_pose_, self.trolley_pose_) = (None, None)
self.displayParameters()
self.initClients()
self.initServices()
self.initServers()
self.initSubscribers()
self.initTopics()
def confirm(self):
if not self.confirm_:
return
raw_input("Please press enter to confirm")
def displayParameters(self):
if not self.verbose_:
return
print("========== baker_arm_server Parameters ==========")
print("todo")
@log
def Initialize(self):
self.sss.init('gripper')
self.sss.init('arm')
@log
def initClients(self):
# Warning: wait_for_server and wait_for_service are not the same function.
# * rospy.duration expected for wait_for_server
# * float expected for wait_for_service
self.plan_client_ = SimpleActionClient('/arm_planning_node/PlanTo',
PlanToAction)
self.plan_client_.wait_for_server(timeout=rospy.Duration(5.0))
self.execution_client_ = SimpleActionClient('/traj_exec',
ExecuteTrajectoryAction)
self.execution_client_.wait_for_server(timeout=rospy.Duration(5.0))
self.small_gripper_finger_client_ = rospy.ServiceProxy(
"/gripper/driver/set_object", SetObject)
# rmb-ma. keep commented
# self.small_gripper_finger_client_.wait_for_service(timeout=5.0)
self.add_collision_object_client_ = rospy.ServiceProxy(
"/ipa_planning_scene_creator/add_collision_objects",
AddCollisionObject)
self.add_collision_object_client_.wait_for_service(timeout=5.0)
self.attach_object_client_ = rospy.ServiceProxy(
"ipa_planning_scene_creator/attach_object", AddCollisionObject)
self.attach_object_client_.wait_for_service(timeout=5.0)
self.detach_object_client_ = rospy.ServiceProxy(
"ipa_planning_scene_creator/detach_object", RemoveCollisionObject)
self.detach_object_client_.wait_for_service(timeout=5.0)
@log
def initServers(self):
self.to_joints_position_server_ = SimpleActionServer(
self.name_ + '/set_joints_values', ExecuteTrajectoryAction,
self.moveToJointsPositionCallback, False)
self.to_joints_position_server_.start()
self.to_transport_position_server_ = SimpleActionServer(
self.name_ + '/transport_position', MoveToAction,
self.moveToTransportPositionCallback, False)
self.to_transport_position_server_.start()
self.to_rest_position_server_ = SimpleActionServer(
self.name_ + '/rest_position', MoveToAction,
self.moveToRestPositionCallback, False)
self.to_rest_position_server_.start()
self.catch_trashcan_server_ = SimpleActionServer(
self.name_ + '/catch_trashcan', MoveToAction,
self.catchTrashcanCallback, False)
self.catch_trashcan_server_.start()
self.leave_trashcan_server_ = SimpleActionServer(
self.name_ + '/leave_trashcan', MoveToAction,
self.leaveTrashcanCallback, False)
self.leave_trashcan_server_.start()
self.empty_trashcan_server_ = SimpleActionServer(
self.name_ + '/empty_trashcan', MoveToAction,
self.emptyTrashcanCallback, False)
self.empty_trashcan_server_.start()
@log
def initServices(self):
pass
@log
def initTopics(self):
Thread(target=self.statusTalker).start()
@log
def statusTalker(self):
publisher = rospy.Publisher(self.name_ + '/status',
Int32,
queue_size=10)
rate = rospy.Rate(5)
while not rospy.is_shutdown():
publisher.publish(self.status_.value)
rate.sleep()
@log
def initSubscribers(self):
rospy.Subscriber("/arm/joint_states", JointState,
self.jointStateCallback)
def jointStateCallback(self, msg):
'''
getting current position of the joint
@param msg containts joint position, velocity, effort, names
'''
for i in range(0, len(msg.position)):
self.joint_values_[i] = msg.position[i]
def gripperHandler(self,
finger_value=0.01,
finger_open=False,
finger_close=True):
'''
Gripper handler function use to control gripper open/close command,
move parallel part of gripper with give values (in cm)
@param finger_value is the big parallel part move realtive to zero position
@finger_open open the small finger
@finger_close close the small finger
'''
if finger_open or finger_close:
# set_object request, both the finger move together (at the same time)
request = SetObjectRequest()
response = SetObjectResponse()
request.node = 'gripper_finger1_joint'
request.object = '22A0'
if finger_open:
request.value = '5'
elif finger_close:
request.value = '10'
request.cached = False
response = self.small_gripper_finger_client_.call(request)
if response.success:
rospy.loginfo(response.message)
else:
rospy.logerr(response.message)
rospy.logerr("SetObject service call Failed!!")
# default blocking is true, wait until if finishes it tasks
self.sss.move('gripper', [[finger_value]])
return response.success
@log
def handleCheckAccessibility(self, request):
response = TriggerResponse()
response.success = True
return response
@log
def openGripper(self):
if not self.IS_GRIPPER_AVAILABLE:
return
self.gripperHandler(finger_value=0.01,
finger_open=True,
finger_close=False)
@log
def closeGripper(self):
if not self.IS_GRIPPER_AVAILABLE:
return
self.gripperHandler(finger_value=-0.01,
finger_open=False,
finger_close=True)
@log
def executeTrajectory(self, trajectory, server):
goal = ExecuteTrajectoryGoal()
goal.trajectory = trajectory
self.execution_client_.send_goal(goal)
while self.execution_client_.get_state() < 3:
rospy.sleep(0.1)
if not server.is_preempt_requested() and not rospy.is_shutdown():
continue
self.execution_client_.cancel_goal()
self.execution_client_.wait_for_result()
return True
result = self.execution_client_.get_result()
state = self.execution_client_.get_state()
if result.success and state == 3:
rospy.logwarn("Execution finish with given time")
return True
else:
rospy.logerr("Execution aborted!!")
return False
@log
def isPoseAccessible(self, target_pose):
(_, is_planned) = planTrajectoryInCartSpace(client=self.plan_client_,
object_pose=target_pose,
bdmp_goal=None,
bdmp_action='')
return is_planned
@log
def planAndExecuteTrajectoryInJointSpaces(self, target_joints, server):
(trajectory,
is_planned) = planTrajectoryInJointSpace(client=self.plan_client_,
object_pose=target_joints,
bdmp_goal=None,
bdmp_action='')
if not is_planned:
rospy.logerr('Trajectory in joint spaces execution failed')
raise Exception('Trajectory in joint spaces execution failed')
self.confirm()
is_execution_successful = self.executeTrajectory(trajectory=trajectory,
server=server)
if not is_execution_successful:
rospy.logerr('Can not find valid trajectory at joint space')
raise Exception('Trajectory Execution failed')
return not server.is_preempt_requested()
@log
def planAndExecuteTrajectoryInCartesianSpace(self, target_pose, server):
(trajectory,
is_planned) = planTrajectoryInCartSpace(client=self.plan_client_,
object_pose=target_pose,
bdmp_goal=None,
bdmp_action='')
if not is_planned:
rospy.logerr('Cannot find valid trajectory at cartesianSpace')
raise Exception('Cannot find valid trajectory at cartesian space')
self.confirm()
is_execution_successful = self.executeTrajectory(trajectory=trajectory,
server=server)
if not is_execution_successful:
rospy.logerr("Trajectory Execution Failed")
raise Exception('Trajectory Execution Failed')
return not server.is_preempt_requested()
@staticmethod
def poseToLists(pose):
position = [pose.position.x, pose.position.y, pose.position.z]
orientation = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
return (position, orientation)
@log
def catchTrashcanCallback(self, goal):
rospy.sleep(3)
result = MoveToResult()
result.arrived = False
target_pose = goal.target_pos
target_pose.pose.position.z *= 1.1
#The arm canot carry a new trashcan as it already carries one
if self.status_ != ArmStatus.NO_TRASHCAN:
self.catch_trashcan_server_.set_aborted(result)
try:
self.planAndExecuteTrajectoryInCartesianSpace(
target_pose, self.catch_trashcan_server_)
except Exception as e:
print(e)
self.catch_trashcan_server_.set_aborted(result)
return
if self.catch_trashcan_server_.is_preempt_requested():
result.arrived = True
self.catch_trashcan_server_.set_preempted(result)
return
self.closeGripper()
#self.fixTrashCanToRobot()
self.status_ = ArmStatus.FULL_TRASHCAN
# todo rmb-ma setup the grapping move with a real trashcan
# Help:
# try:
# target_pose = make_pose(position=[0.00,-0.00,-0.07], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper')
# self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_)
# except:
# self.catch_trashcan_server_.set_aborted(result)
# return
#
# if self.catch_trashcan_server_.is_preempt_requested():
# result.arrived = True
# self.catch_trashcan_server_.set_preempted(result)
# return
#
# try:
# target_pose = make_pose(position=[0.020,-0.00,0.00], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper')
# self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_)
# except:
# self.catch_trashcan_server_.set_aborted(result)
# return
#
# if self.catch_trashcan_server_.is_preempt_requested():
# result.arrived = True
# self.catch_trashcan_server_.set_preempted(result)
# return
#
# try:
# target_pose = make_pose(position=[0.000,-0.00,0.030], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper')
# self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_)
# except:
# self.catch_trashcan_server_.set_aborted(result)
# return
#
result.arrived = True
self.catch_trashcan_server_.set_succeeded(result)
@log
def emptyTrashcanCallback(self, goal):
result = MoveToResult()
#Trashcan emptying while the arm doesnt carry any trashcan
if self.status_ != ArmStatus.FULL_TRASHCAN:
result.arrived = False
self.empty_trashcan_server_.set_aborted(result)
return
try:
self.planAndExecuteTrajectoryInCartesianSpace(
goal.target_pos, self.empty_trashcan_server_)
except:
result.arrived = False
self.empty_trashcan_server_.set_aborted(result)
return
if self.empty_trashcan_server_.is_preempt_requested():
self.empty_trashcan_server_.set_preempted()
return
try:
target_joints = self.joint_values_[0:-1] + [3.]
self.planAndExecuteTrajectoryInJointSpaces(
target_joints, self.empty_trashcan_server_)
except:
result.arrived = False
self.empty_trashcan_server_.set_aborted(result)
return
if self.empty_trashcan_server_.is_preempt_requested():
self.empty_trashcan_server_.set_preempted()
return
rospy.sleep(5)
self.status_ = ArmStatus.FULL_TRASHCAN
target_joints = self.joint_values_[0:-1] + [0]
self.planAndExecuteTrajectoryInJointSpaces(target_joints,
self.empty_trashcan_server_)
result.arrived = True
self.empty_trashcan_server_.set_succeeded(result)
@log
def moveToRestPositionCallback(self, goal):
result = MoveToResult()
# Rest position is unavailable if the arm carries a trashcan
if self.status_ != ArmStatus.NO_TRASHCAN:
result.arrived = False
self.to_rest_position_server_.set_aborted(result)
return
try:
self.planAndExecuteTrajectoryInJointSpaces(
self.DEFAULT_POSITION, self.to_rest_position_server_)
except Exception as e:
rospy.logerr(e)
result.arrived = False
self.to_rest_position_server_.set_aborted(result)
return
if self.to_rest_position_server_.is_preempt_requested():
self.to_rest_position_server_.set_preempted()
return
self.to_rest_position_server_.set_succeeded()
@log
def moveToTransportPositionCallback(self, goal):
result = MoveToResult()
# Transport position is unavailable if the arm doesnt carry a trashcan
if self.status_ == ArmStatus.NO_TRASHCAN:
result.arrived = False
self.to_transport_position_server_.set_aborted(result)
return
try:
result.arrived = self.planAndExecuteTrajectoryInJointSpaces(
self.TRANSPORT_POSITION, self.to_transport_position_server_)
except:
result.arrived = False
self.to_transport_position_server_.set_aborted(result)
return
if self.to_transport_position_server_.is_preempt_requested():
self.to_transport_position_server_.set_preempted()
return
self.to_transport_position_server_.set_succeeded(result)
@log
def leaveTrashcanCallback(self, goal):
result = MoveToResult()
# The arm cannot leave a trashcan if it doesnt carry one
if self.status_ != ArmStatus.EMPTY_TRASHCAN:
result.arrived = False
self.leave_trashcan_server_.set_aborted(result)
return
self.planAndExecuteTrajectoryInCartesianSpace(
goal.target_pos, self.leave_trashcan_server_)
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
self.openGripper()
self.status_ = ArmStatus.NO_TRASHCAN
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
target_pose = make_pose(position=[-0.07, -0., -0.04],
orientation=[0., 0., 0., 1.],
frame_id='gripper')
self.planAndExecuteTrajectoryInCartesianSpace(
target_pose, self.leave_trashcan_server_)
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
target_pose = make_pose(position=[-0.0, -0., 0.05],
orientation=[0., 0., 0., 1.],
frame_id='gripper')
self.planAndExecuteTrajectoryInCartesianSpace(
target_pose, self.leave_trashcan_server_)
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
result.arrived = True
self.leave_trashcan_server_.set_succeeded(result)
@log
def moveToJointsPositionCallback(self, goal):
target_joints = goal.trajectory.joint_trajectory.points[0].positions
self.planAndExecuteTrajectoryInJointSpaces(
target_joints, self.to_joints_position_server_)
if self.to_joints_position_server_.is_preempt_requested():
self.to_joints_position_server_.set_preempted()
return
self.to_joints_position_server_.set_succeeded()
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 SmartArmActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 4 # 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_pan_link'
self.shoulder_pan = JointControllerState(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', JointControllerState, self.read_shoulder_pan)
rospy.wait_for_message('shoulder_pan_controller/state', JointControllerState)
# Initialize publisher & subscriber for shoulder tilt
self.shoulder_tilt_frame = 'arm_shoulder_tilt_link'
self.shoulder_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.shoulder_tilt_pub = rospy.Publisher('shoulder_tilt_controller/command', Float64)
rospy.Subscriber('shoulder_tilt_controller/state', JointControllerState, self.read_shoulder_tilt)
rospy.wait_for_message('shoulder_tilt_controller/state', JointControllerState)
# Initialize publisher & subscriber for elbow tilt
self.elbow_tilt_frame = 'arm_elbow_tilt_link'
self.elbow_tilt = JointControllerState(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', JointControllerState, self.read_elbow_tilt)
rospy.wait_for_message('elbow_tilt_controller/state', JointControllerState)
# Initialize publisher & subscriber for shoulder tilt
self.wrist_rotate_frame = 'arm_wrist_rotate_link'
self.wrist_rotate = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_rotate_pub = rospy.Publisher('wrist_rotate_controller/command', Float64)
rospy.Subscriber('wrist_rotate_controller/state', JointControllerState, self.read_wrist_rotate)
rospy.wait_for_message('wrist_rotate_controller/state', JointControllerState)
# Initialize tf listener
self.tf = tf.TransformListener()
# Initialize joints action server
self.result = SmartArmResult()
self.feedback = SmartArmFeedback()
self.feedback.arm_position = [self.shoulder_pan.process_value, self.shoulder_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_rotate.process_value]
self.server = SimpleActionServer(NAME, SmartArmAction, 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.shoulder_tilt_pub.publish(1.972222)
self.elbow_tilt_pub.publish(-1.972222)
self.wrist_rotate_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_shoulder_tilt(self, tilt_data):
self.shoulder_tilt = tilt_data
self.has_latest_shoulder_tilt = True
def read_elbow_tilt(self, tilt_data):
self.elbow_tilt = tilt_data
self.has_latest_elbow_tilt = True
def read_wrist_rotate(self, rotate_data):
self.wrist_rotate = rotate_data
self.has_latest_wrist_rotate = True
def wait_for_latest_controller_states(self, timeout):
self.has_latest_shoulder_pan = False
self.has_latest_shoulder_tilt = False
self.has_latest_elbow_tilt = False
self.has_latest_wrist_rotate = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_shoulder_pan == False or self.has_latest_shoulder_tilt == False or \
self.has_latest_elbow_tilt == False or self.has_latest_wrist_rotate == 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.shoulder_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_rotate.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.shoulder_tilt_pub.publish(target_joints[1])
self.elbow_tilt_pub.publish(target_joints[2])
self.wrist_rotate_pub.publish(target_joints[3])
# 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.shoulder_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_rotate.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.shoulder_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_rotate.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.shoulder_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_rotate.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 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 FakeGrasping:
ALWAYS = 0
NEVER = 1
RANDOM = 2
def __init__(self):
self.server = SimpleActionServer('/robot_move',
RobotMoveAction,
execute_cb=self.robot_move_cb)
self.objects = self.ALWAYS
self.grasp = self.ALWAYS
self.place = self.ALWAYS
self.object_randomness = 0.8 # 80% of time object is known
self.grasp_randomness = 0.4
self.place_randomness = 0.4
self.holding = None
self.pick_length = 2 # how long (sec) takes to pick an object
self.place_length = 2 # how long (sec) takes to place an object
self.robot_state_pub = rospy.Publisher("/robot_status",
RobotStatus,
queue_size=1,
latch=True)
self.publish_robot_status(RobotStatus.STOPPED)
random.seed()
def publish_robot_status(self, status):
self.robot_state_pub.publish(
RobotStatus(
status=status,
actual_pose=PoseStamped(header=Header(frame_id="marker"))))
def robot_move_cb(self, goal):
self.publish_robot_status(RobotStatus.MOVING)
self.robot_move(goal)
self.publish_robot_status(RobotStatus.STOPPED)
def robot_move(self, goal):
result = RobotMoveResult()
feedback = RobotMoveFeedback()
if not (goal.move_type in (goal.HOME, goal.PROGRAMING, goal.PICK,
goal.PLACE, goal.MOVE)):
result.result = RobotMoveResult.BAD_REQUEST
rospy.logerr("BAD_REQUEST, Unknown operation")
self.server.set_aborted(result, "Unknown operation")
return
if self.objects == self.ALWAYS:
pass
elif self.objects == self.NEVER:
result.result = RobotMoveResult.BAD_REQUEST
rospy.logerr("BAD_REQUEST, Unknown object id")
self.server.set_aborted(result, "Unknown object id")
return
elif self.objects == self.RANDOM:
nmbr = random.random()
if nmbr > self.object_randomness:
result.result = RobotMoveResult.BAD_REQUEST
rospy.logerr("BAD_REQUEST, Unknown object id")
self.server.set_aborted(result, "Unknown object id")
return
grasped = False
if goal.move_type == RobotMoveGoal.PICK:
rospy.sleep(self.pick_length)
if False and self.holding:
result.result = RobotMoveResult.BUSY
rospy.logerr("Failure, already holding object in arm")
self.server.set_aborted(result,
"Already holding object in arm")
return
if self.grasp == self.ALWAYS:
grasped = True
pass
elif self.grasp == self.NEVER:
result.result = RobotMoveResult.MOVE_FAILURE
rospy.logerr("FAILURE, Pick Failed")
self.server.set_aborted(result, "Pick Failed")
return
tries = 5
while tries > 0:
feedback.state = feedback.PICKING
tries -= 1
self.server.publish_feedback(feedback)
if self.grasp == self.RANDOM:
nmbr = random.random()
if nmbr < self.grasp_randomness:
grasped = True
if grasped:
break
if self.server.is_preempt_requested():
self.server.set_preempted(result, "Pick canceled")
rospy.logerr("Preempted")
return
if not grasped:
result.result = RobotMoveResult.FAILURE
self.server.set_aborted(result, "Pick failed")
rospy.logerr("FAILURE, Pick Failed")
return
else:
self.holding = True
placed = False
if goal.move_type == RobotMoveGoal.PLACE:
rospy.sleep(self.place_length)
if not self.holding:
result.result = RobotMoveResult.MOVE_FAILURE
rospy.logerr("Failure, robot not holding object")
self.server.set_aborted(result, "Robot not holding object")
return
if self.place == self.ALWAYS:
placed = True
pass
elif self.place == self.NEVER:
result.result = RobotMoveResult.MOVE_FAILURE
self.server.set_aborted(result, "Place Failed")
rospy.logerr("FAILURE, Place Failed")
return
tries = 5
while tries > 0:
feedback.state = feedback.PLACING
tries -= 1
self.server.publish_feedback(feedback)
if self.place == self.RANDOM:
nmbr = random.random()
if nmbr < self.place_randomness:
placed = True
if placed:
break
if not placed:
result.result = RobotMoveResult.MOVE_FAILURE
self.server.set_aborted(result, "Place failed")
rospy.logerr("FAILURE, Place Failed")
return
else:
self.holding = False
result.result = RobotMoveResult.SUCCES
self.server.set_succeeded(result)
rospy.loginfo("SUCCESS")
print("Finished")
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 PathExecutor:
_continue=True
_skip_unreachable=True
_path_result=ExecutePathResult()
def __init__(self):
self._poses = []
self._poses_idx = 0
self._client = SimpleActionClient('/bug2/move_to', MoveToAction)
self._action_name = rospy.get_name() + "/execute_path"
self._server = SimpleActionServer(self._action_name, ExecutePathAction, execute_cb=self.execute_cb, auto_start = False)
self._server.start()
def execute_cb(self, goal):
"""
This funciton is used to execute the move to goal
"""
self._path_result.visited=[] #Initialize the path result
self._skip_unreachable=goal.skip_unreachable
self._client.wait_for_server()
self._poses = goal.path.poses
self._poses_idx = 0 #Start with the first goal
self._continue= True
rospy.loginfo('Starting Number of Poses: %s'%len(self._poses))
move = MoveToGoal()
move.target_pose.pose = self._poses[self._poses_idx].pose
"""
Send the goal to the _client and once done, go to callback function
"move_to_done_cb"
"""
self._client.send_goal(move, done_cb=self.move_to_done_cb)
while (self._continue==True):
"""
Check if current goal is preempted by other user action.
If so, break out of loop
"""
if self._server.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._server.set_preempted()
self._continue= False
break
def move_to_done_cb(self, state, result):
"""
This call back function is called once one goal is completed.
This checks if there are any other goal remaining to be executed and
recurrsively calls itself.
"""
feedback = ExecutePathFeedback()
feedback.pose=self._poses[self._poses_idx]
"""
#======================================================================
#* If state is 3, i.e., goal was successful, publish feedback
# and move to next goal
#
#* Else publish feedback and check _skip_unreachable flag,
# * If True go to next pose
# * Else break
#======================================================================
"""
if(state == 3):
feedback.reached = True
self._path_result.visited.append(feedback.reached)
self._server.publish_feedback(feedback)
self._poses_idx = self._poses_idx + 1
"""
* If more pose available, move to next goal
* Else break
"""
if(self._poses_idx < len(self._poses)):
move = MoveToGoal()
move.target_pose.pose = self._poses[self._poses_idx].pose
self._client.send_goal(move, done_cb=self.move_to_done_cb)
else:
self._continue=False
self._server.set_succeeded(self._path_result)
else:
feedback.reached = False
self._path_result.visited.append(False)
self._server.publish_feedback(feedback)
if(self._skip_unreachable==True):
self._poses_idx = self._poses_idx + 1
"""
* If more pose available, move to next goal
* Else break
"""
if(self._poses_idx < len(self._poses)):
move = MoveToGoal()
move.target_pose.pose = self._poses[self._poses_idx].pose
self._client.send_goal(move, done_cb=self.move_to_done_cb)
else:
self._continue=False
self._server.set_succeeded(self._path_result)
else:
rospy.loginfo('Unreachable')
self._continue=False
self._server.set_succeeded(self._path_result)
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 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 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 ErraticBaseActionServer():
def __init__(self):
self.base_frame = '/base_footprint'
self.move_client = SimpleActionClient('move_base', MoveBaseAction)
self.move_client.wait_for_server()
self.tf = tf.TransformListener()
self.result = ErraticBaseResult()
self.feedback = ErraticBaseFeedback()
self.server = SimpleActionServer(NAME, ErraticBaseAction, self.execute_callback, auto_start=False)
self.server.start()
rospy.loginfo("%s: Ready to accept goals", NAME)
def transform_target_point(self, point):
self.tf.waitForTransform(self.base_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
return self.tf.transformPoint(self.base_frame, point)
def move_to(self, target_pose):
goal = MoveBaseGoal()
goal.target_pose = target_pose
goal.target_pose.header.stamp = rospy.Time.now()
self.move_client.send_goal(goal=goal, feedback_cb=self.move_base_feedback_cb)
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()
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)
def get_vicinity_target(self, target_pose, vicinity_range):
vicinity_pose = PoseStamped()
# transform target to base_frame reference
target_point = PointStamped()
target_point.header.frame_id = target_pose.header.frame_id
target_point.point = target_pose.pose.position
self.tf.waitForTransform(self.base_frame, target_pose.header.frame_id, rospy.Time(), rospy.Duration(5.0))
target = self.tf.transformPoint(self.base_frame, target_point)
rospy.logdebug("%s: Target at (%s, %s, %s)", NAME, target.point.x, target.point.y, target.point.z)
# find distance to point
dist = math.sqrt(math.pow(target.point.x, 2) + math.pow(target.point.y, 2))
if (dist < vicinity_range):
# if already within range, then no need to move
vicinity_pose.pose.position.x = 0.0
vicinity_pose.pose.position.y = 0.0
else:
# normalize vector pointing from source to target
target.point.x /= dist
target.point.y /= dist
# scale normal vector to within vicinity_range distance from target
target.point.x *= (dist - vicinity_range)
target.point.y *= (dist - vicinity_range)
# add scaled vector to source
vicinity_pose.pose.position.x = target.point.x + 0.0
vicinity_pose.pose.position.y = target.point.y + 0.0
# set orientation
ori = Quaternion()
yaw = math.atan2(target.point.y, target.point.x)
(ori.x, ori.y, ori.z, ori.w) = tf.transformations.quaternion_from_euler(0, 0, yaw)
vicinity_pose.pose.orientation = ori
# prep header
vicinity_pose.header = target_pose.header
vicinity_pose.header.frame_id = self.base_frame
rospy.logdebug("%s: Moving to (%s, %s, %s)", NAME, vicinity_pose.pose.position.x, vicinity_pose.pose.position.y, vicinity_pose.pose.position.z)
return vicinity_pose
def execute_callback(self, goal):
rospy.loginfo("%s: Executing move base", NAME)
move_base_result = None
if goal.vicinity_range == 0.0:
# go to exactly
move_base_result = self.move_to(goal.target_pose)
else:
# go near (within vicinity_range meters)
vicinity_target_pose = self.get_vicinity_target(goal.target_pose, goal.vicinity_range)
move_base_result = self.move_to(vicinity_target_pose)
# check results
if (move_base_result == GoalStatus.SUCCEEDED):
rospy.loginfo("%s: Succeeded", NAME)
self.result.base_position = self.feedback.base_position
self.server.set_succeeded(self.result)
elif (move_base_result == GoalStatus.PREEMPTED):
rospy.loginfo("%s: Preempted", NAME)
self.server.set_preempted()
else:
rospy.loginfo("%s: Aborted", NAME)
self.server.set_aborted()
