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 OmniPoseFollower(object):
def __init__(self):
self.server = SimpleActionServer(
'/whole_body_controller/refills_finger/follow_joint_trajectory',
FollowJointTrajectoryAction,
self.execute_cb,
auto_start=False)
self.state_pub = rospy.Publisher('refills_finger/state',
JointTrajectoryControllerState,
queue_size=10)
self.js_sub = rospy.Subscriber('refills_finger/joint_states',
JointState,
self.js_cb,
queue_size=10)
self.server.start()
def js_cb(self, data):
msg = JointTrajectoryControllerState()
msg.joint_names = data.name
self.state_pub.publish(msg)
def execute_cb(self, data):
"""
:type data: FollowJointTrajectoryGoal
:return:
"""
self.server.set_succeeded()
class MoveBase(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.odom_topic = rospy.get_param("~odom_topic",
"/naoqi_driver_node/odom")
self.listener = tf.TransformListener()
self.robot_pose = None
rospy.Subscriber(self.odom_topic, Odometry, self.odom_cb)
self.target_frame = rospy.get_param("~target_frame", "base_link")
# self.pub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=10)
self._as = SimpleActionServer(name,
MoveBaseAction,
self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("... done")
def __call_service(self, srv_name, srv_type, req):
while not rospy.is_shutdown():
try:
s = rospy.ServiceProxy(srv_name, srv_type)
s.wait_for_service(timeout=1.)
except rospy.ROSException, rospy.ServiceException:
rospy.logwarn(
"Could not communicate with '%s' service. Retrying in 1 second."
% srv_name)
rospy.sleep(1.)
else:
return s(req)
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 TrajectoryExecution:
def __init__(self, side_prefix):
traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints)
self.action_server.start()
self.has_goal = False
def move_to_joints(self, traj_goal):
rospy.loginfo('Receieved a trajectory execution request.')
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
self.traj_action_client.send_goal(traj_goal)
rospy.sleep(1)
is_terminated = False
while(not is_terminated):
action_state = self.traj_action_client.get_state()
if (action_state == GoalStatus.SUCCEEDED):
self.action_server.set_succeeded()
is_terminated = True
elif (action_state == GoalStatus.ABORTED):
self.action_server.set_aborted()
is_terminated = True
elif (action_state == GoalStatus.PREEMPTED):
self.action_server.set_preempted()
is_terminated = True
rospy.loginfo('Trajectory completed.')
class axGripperServer:
def __init__(self, name):
self.fullname = name
self.goalAngle = 0.0
self.actualAngle = 0.0
self.failureState = False
dynamixelChain.move_angles_sync(ids[6:], [self.goalAngle], [0.5])
self.server = SimpleActionServer(
self.fullname, GripperCommandAction, execute_cb=self.execute_cb, auto_start=False
)
self.server.start()
def execute_cb(self, goal):
rospy.loginfo(goal)
self.currentAngle = goal.command.position
dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5])
attempts = 0
for i in range(10):
rospy.sleep(0.1)
attempts += 1
# while ... todo: add some condition to check on the actual angle
# rospy.sleep(0.1)
# print jPositions[4]
# attempts += 1
if attempts < 20:
self.server.set_succeeded()
else:
self.server.set_aborted()
def checkFailureState(self):
if self.failureState:
print "I am currently in a failure state."
class GiveVoucher(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._as = SimpleActionServer(name,
GiveVoucherAction,
self.execute_cb,
auto_start=False)
self.logo_app = rospy.get_param("~logo_app",
"showmummerlogo-a897b8/behavior_1")
client = MongoClient(rospy.get_param("~db_host", "localhost"),
int(rospy.get_param("~db_port", 62345)))
self.db_name = rospy.get_param("~db_name", "semantic_map")
self.db = client[self.db_name]
self.collection_name = rospy.get_param("~collection_name", "idea_park")
self.semantic_map_name = rospy.get_param("~semantic_map_name")
self._as.start()
rospy.loginfo("... done")
def __call_service(self, srv_name, srv_type, req):
while not rospy.is_shutdown():
try:
s = rospy.ServiceProxy(srv_name, srv_type)
s.wait_for_service(timeout=1.)
except rospy.ROSException, rospy.ServiceException:
rospy.logwarn(
"Could not communicate with '%s' service. Retrying in 1 second."
% srv_name)
rospy.sleep(1.)
else:
return s(req)
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 axGripperServer:
def __init__(self, name):
self.fullname = name
self.currentAngle = 0.0
# dynamixelChain.move_angle(7, 0.0, 0.5)
dynamixelChain.move_angles_sync(ids[6:], [0.0], [0.5])
self.server = SimpleActionServer(self.fullname,
GripperCommandAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def execute_cb(self, goal):
rospy.loginfo(goal)
self.currentAngle = goal.command.position
dynamixelChain.move_angles_sync(ids[6:], [self.currentAngle], [0.5])
# dynamixelChain.move_angle(7, 0.1, 0.5)
rospy.sleep(0.1)
print jPositions[4]
attempts = 0
while abs(goal.command.position - jPositions[4]) > 0.1 and attempts < 20:
rospy.sleep(0.1)
print jPositions[4]
attempts += 1
if attempts < 20:
self.server.set_succeeded()
else:
self.server.set_aborted()
class MockActionServer(object):
""" MockActionServer base class """
def __init__(self, name, topic, action_type):
""" Creating a custom mock action server."""
self._topic = topic
self._name = name
self._action_type = action_type
self.timeout = 5
self.action_result = None
Subscriber('mock/' + name, String, self.receive_commands)
Subscriber('mock/gui_result', Bool, self.set_gui_result)
self._server = ActionServer(self._topic, self._action_type,
self.success, False)
self._server.start()
loginfo('>>> Starting ' + self._name)
def receive_commands(self, msg):
""" Decides the result of the next call. """
callback, timeout = msg.data.split(':')
self.timeout = float(timeout)
self._server.execute_callback = getattr(self, callback)
logwarn('>>> ' + self._name + ': Current callback -> ' + callback)
sleep(1)
def abort(self, goal):
""" Aborts any incoming goal. """
logwarn('>>> ' + self._name + ': This goal will be aborted.')
sleep(self.timeout)
self._server.set_aborted()
def success(self, goal):
""" Succeeds any incoming goal. """
logwarn('>>> ' + self._name + ': This goal will succeed.')
sleep(self.timeout)
self._server.set_succeeded(self.action_result)
def preempt(self, goal):
""" Preempts any incoming goal. """
logwarn('>>> ' + self._name + ': This goal will be preempted.')
sleep(self.timeout)
self._server.set_preempted()
def set_gui_result(self, msg):
""" Sets the result of the goal. """
self.action_result = ValidateVictimGUIResult()
logwarn('>>> The gui response will be: ' + str(msg.data))
self.action_result.victimValid = msg.data
class 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 MockActionServer(object):
""" MockActionServer base class """
def __init__(self, name, topic, action_type):
""" Creating a custom mock action server."""
self._topic = topic
self._name = name
self._action_type = action_type
self.timeout = 1
self.action_result = None
self.prefered_callback = ' '
Subscriber('mock/' + name, String, self.receive_commands)
Subscriber('mock/gui_result', Bool, self.set_gui_result)
self._server = ActionServer(self._topic, self._action_type,
self.decide, False)
self._server.start()
loginfo('>>> Starting ' + self._name)
def receive_commands(self, msg):
""" Decides the result of the next call. """
callback, timeout = msg.data.split(':')
# self.timeout = float(timeout)
self.prefered_callback = callback
sleep(1)
def decide(self, goal):
""" Deciding outcome of goal """
logwarn('Deciding callback...')
sleep(self.timeout)
if(self.prefered_callback == 'success'):
logwarn('>>> ' + self._name + ': This goal will succeed.')
self._server.set_succeeded()
elif(self.prefered_callback == 'abort'):
logwarn('>>> ' + self._name + ': This goal will be aborted.')
self._server.set_aborted()
elif(self.prefered_callback == 'preempt'):
logwarn('>>> ' + self._name + ': This goal will be preempted.')
self._server.set_preempted()
else:
logwarn('wtf?')
def set_gui_result(self, msg):
""" Sets the result of the goal. """
self.action_result = ValidateVictimGUIResult()
logwarn('>>> The gui response will be: ' + str(msg.data))
self.action_result.victimValid = msg.data
class ScanTableActionServer:
"""
Scan Table Mock
Run this file to have mock to the action '/head_traj_controller/head_scan_snapshot_action '(Scan Table)
"""
def __init__(self):
a_scan_table = {'name': '/head_traj_controller/head_scan_snapshot_action', 'ActionSpec': PointHeadAction, 'execute_cb': self.scan_table_cb, 'auto_start': False}
self.s = SimpleActionServer(**a_scan_table)
self.s.start()
def scan_table_cb(self, req):
rospy.loginfo('Scan Table \'/head_traj_controller/head_scan_snapshot_action was called.')
self.s.set_succeeded() if bool(random.randint(0, 1)) else self.s.set_aborted()
class AveragingSVR2(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.register_goal_callback(self.goal_cb)
self.reset_numbers()
rospy.Subscriber('number', Float32, self.execute_loop)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def goal_cb(self):
self._goal = self._action.accept_new_goal()
rospy.loginfo('goal callback %s' % (self._goal))
def preempt_cb(self):
rospy.loginfo('preempt callback')
self.reset_numbers()
self._action.set_preempted(text='message for preempt')
def reset_numbers(self):
self._samples = []
def execute_loop(self, msg):
if (not self._action.is_active()):
return
self._samples.append(msg.data)
feedback = AveragingAction().action_feedback.feedback
feedback.sample = len(self._samples)
feedback.data = msg.data
feedback.mean = sum(self._samples) / len(self._samples)
feedback.std_dev = self.std_dev(self._samples)
self._action.publish_feedback(feedback)
## sending result
if (len(self._samples) >= self._goal.samples):
result = AveragingAction().action_result.result
result.mean = sum(self._samples) / len(self._samples)
result.std_dev = self.std_dev(self._samples)
rospy.loginfo('result: %s' % (result))
self.reset_numbers()
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result,
text='message for aborted')
class Turtle_Server:
def __init__(self):
rospy.loginfo("inside __init__")
self.Server = "/turtle_thing"
print(self.Server)
self.turtle_server = SimpleActionServer(self.Server,
Turtle_positionAction,
execute_cb=self.execute_cb,
auto_start=False)
self.turtle_server.start()
def execute_cb(self, goal):
rospy.loginfo("inside execute_cb")
print(goal.x, goal.y, goal.theta)
class ybGripperServer:
def __init__(self, name):
self.fullname = name
self.currentValue = 0.0
gripperTopic = '/arm_1/gripper_controller/position_command'
self.jppub = rospy.Publisher(gripperTopic, JointPositions)
self.server = SimpleActionServer(self.fullname,
GripperCommandAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def execute_cb(self, goal):
rospy.loginfo(goal)
self.currentValue = goal.command.position
self.moveGripper(self.jppub, self.currentValue)
attempts = 0
# here we should be checking if the gripper has gotten to its goal
for i in range(5):
rospy.sleep(0.1)
attempts += 1
if attempts < 20:
self.server.set_succeeded()
else:
self.server.set_aborted()
def moveGripper(self, gPublisher, floatVal):
jp = JointPositions()
myPoison = Poison()
myPoison.originator = 'yb_grip'
myPoison.description = 'whoknows'
myPoison.qos = 0.0
jp.poisonStamp = myPoison
nowTime = rospy.Time.now()
jvl = JointValue()
jvl.timeStamp = nowTime
jvl.joint_uri = 'gripper_finger_joint_l'
jvl.unit = 'm'
jvl.value = floatVal
jp.positions.append(jvl)
jvr = JointValue()
jvr.timeStamp = nowTime
jvr.joint_uri = 'gripper_finger_joint_r'
jvr.unit = 'm'
jvr.value = floatVal
jp.positions.append(jvr)
gPublisher.publish(jp)
def ybspin(self):
rospy.sleep(0.1)
class 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 PathExecutor:
def __init__(self, name):
self._action_name = name
self._as = SimpleActionServer(self._action_name,
ExecutePathAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("in action_server")
def execute_cb(self, goal):
pass
def move_to_done_cb(self, state, result):
pass
class GripperController:
def __init__(self, hand):
arm = hand[0]
self.effort = -1.0
self.server = SimpleActionServer(ACTION_NAME % arm,
GripperSequenceAction, self.execute,
False)
self.server.start()
self.sub_client = SimpleActionClient(
"%s_gripper_controller/gripper_action" % arm,
Pr2GripperCommandAction)
#wait for the action servers to come up
rospy.loginfo("[GRIPPER] Waiting for %s controllers" % arm)
self.sub_client.wait_for_server()
rospy.loginfo("[GRIPPER] Got %s controllers" % arm)
self.client = SimpleActionClient(ACTION_NAME % arm,
GripperSequenceAction)
rospy.loginfo("[GRIPPER] Waiting for self client")
self.client.wait_for_server()
rospy.loginfo("[GRIPPER] Got self client")
def send_goal(self, goal):
self.client.send_goal(goal)
def execute(self, goal):
rate = rospy.Rate(10)
while rospy.Time.now() < goal.header.stamp:
rate.sleep()
for position, time in zip(goal.positions, goal.times):
self.change_position(position, False)
rospy.sleep(time)
self.server.set_succeeded(GripperSequenceResult())
self.change_position(position, False, 0.0)
def change_position(self, position, should_wait=True, effort=None):
if effort is None:
effort = self.effort
gg = Pr2GripperCommandGoal()
gg.command.position = position
gg.command.max_effort = effort
self.sub_client.send_goal(gg)
if should_wait:
self.sub_client.wait_for_result()
class TestServerClass():
def __init__(self):
self.smach_action_server = SimpleActionServer('test_smach_action_server',testAction,
execute_cb=self.execute_cb,auto_start=False)
self.smach_action_server.start()
def execute_cb(self,goal):
sm = getStateMachine()
sm.userdata.action_goal = goal
smach_thread = Thread(target=sm.execute)
smach_thread.start()
while(smach_thread.is_alive()):
self.smach_action_server.publish_feedback(testFeedback(str(sm.get_active_states())))
rospy.loginfo(sm.get_active_states())
rospy.sleep(rospy.Duration(1.0))
self.smach_action_server.set_succeeded(testResult("The Task Got Completed"))
class MockGui():
def __init__(self, gui_validation_topic):
self.reply = False
self.preempted = 0
self.victimValid = True
self.victimFoundx = 0
self.victimFoundy = 0
self.probability = 0
self.sensorIDsFound = []
self.gui_validate_victim_as_ = SimpleActionServer(
gui_validation_topic,
ValidateVictimGUIAction,
execute_cb=self.gui_validate_victim_cb,
auto_start=False)
self.gui_validate_victim_as_.start()
def __del__(self):
self.gui_validate_victim_as_.__del__()
def gui_validate_victim_cb(self, goal):
rospy.loginfo('gui_validate_victim_cb')
self.reply = False
self.victimFoundx = goal.victimFoundx
self.victimFoundy = goal.victimFoundy
self.probability = goal.probability
self.sensorIDsFound = goal.sensorIDsFound
print goal
while not self.reply:
rospy.sleep(0.5)
if self.gui_validate_victim_as_.is_preempt_requested():
preempted += 1
self.gui_validate_victim_as_.set_preempted()
break
else:
self.preempted = 0
result = ValidateVictimGUIResult(victimValid=self.victimValid)
self.gui_validate_victim_as_.set_succeeded(result)
class 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 ReemTabletopManipulationMock():
def __init__(self):
a_grasp_target_action = {'name': '/tabletop_grasping_node', 'ActionSpec': GraspTargetAction, 'execute_cb': self.grasp_target_action_cb, 'auto_start': False}
self.s = SimpleActionServer(**a_grasp_target_action)
self.s.start()
def grasp_target_action_cb(self, req):
rospy.loginfo('Grasp Target Action \'%s\' /tabletop_grasping_node was called.' % req.appearanceID)
res = GraspTargetResult()
res.detectionResult = TabletopDetectionResult()
res.detectionResult.models = [DatabaseModelPoseList()]
res.detectionResult.models[0].model_list = [DatabaseModelPose()]# = [0].model_id = req.databaseID
res.detectionResult.models[0].model_list[0].model_id = req.databaseID
self.s.set_succeeded(res) if bool(random.randint(0, 1)) else self.s.set_aborted(res)
class AveragingSVR(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
execute_cb=self.execute_cb,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def preempt_cb(self):
rospy.loginfo('preempt callback')
self._action.set_preempted(text='message for preempt')
def execute_cb(self, goal):
rospy.loginfo('execute callback: %s' % (goal))
feedback = AveragingAction().action_feedback.feedback
result = AveragingAction().action_result.result
## execute loop
rate = rospy.Rate(1 / (0.01 + 0.99 * random.random()))
samples = []
for i in range(goal.samples):
sample = random.random()
samples.append(sample)
feedback.sample = i
feedback.data = sample
feedback.mean = sum(samples) / len(samples)
feedback.std_dev = self.std_dev(samples)
self._action.publish_feedback(feedback)
rate.sleep()
if (not self._action.is_active()):
rospy.loginfo('not active')
return
## sending result
result.mean = sum(samples) / len(samples)
result.std_dev = self.std_dev(samples)
rospy.loginfo('result: %s' % (result))
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result, text='message for aborted')
class 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
self.bugType = bugType.BUG2 # set the bug type to be used
# Service for the Bug algorithm to tell us it is done
self.bug_done_service = rospy.Service('/move_base_fake/is_bug_done/',
SetBool, self.callback_complete)
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()
# publish the state to the bug node, either start or stop.
def pub_state_to_bug(self, state, bugType):
bug_service = bugType.value + '_node/start_stop'
print "sending bug state "
rospy.wait_for_service(bug_service)
try:
# Create a service for the bug service to toggle state
pub_bug_state = rospy.ServiceProxy(bug_service, SetBool)
pub_bug_state(state) # tell bug node to change the exploration
print "sent bug state "
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class SceneDetectionActionServer(object):
__metaclass__ = ABCMeta
def __init__(self, action_name, **kwargs):
rospy.loginfo('broadcasting action server: ' + action_name)
# won't use default auto_start=True as recommended here: https://github.com/ros/actionlib/pull/60
self._action_server = SimpleActionServer(action_name, DetectSceneAction,
execute_cb=self._execute_cb, auto_start=False)
self._initialize(**kwargs)
self._action_server.start()
@abstractmethod
def _initialize(self, **kwargs):
pass
@abstractmethod
def _execute_cb(self, goal):
pass
class OpenDoor():
def __init__(self):
self.pub = rospy.Publisher('/cmd_vel_mux/input/teleop',
Twist,
queue_size=10)
self.sub = rospy.Subscriber('/scan', LaserScan, self.laserscanCB)
self.laser = LaserScan()
self.front_value = 999.9
self._action_server = SimpleActionServer('door_action',
OpenDoorAction,
execute_cb=self.execute,
auto_start=False)
self.flg = False
self.result = OpenDoorResult()
self._action_server.start()
def laserscanCB(self, receive_msg):
self.front_value = receive_msg.ranges[359]
self.flg = True
def linerContorol(self, value):
twist_cmd = Twist()
twist_cmd.linear.x = value
rospy.sleep(0.1)
self.pub.publish(twist_cmd)
def execute(self, goal):
try:
rospy.loginfo('start"door_action"')
while not rospy.is_shutdown() and self.flg == False:
rospy.loginfo('wait for laserscan ...')
rospy.sleep(2.0)
self.flg = False
while not rospy.is_shutdown() and self.front_value < 1.0:
rospy.sleep(1.0)
self.result.data = True
for i in range(10):
self.linerContorol(0.1)
self._action_server.set_succeeded(self.result)
except rospy.ROSInterruptException:
rospy.loginfo('Interrupted')
pass
class SceneDetectionActionServer(object):
__metaclass__ = ABCMeta
_timeout = None # type: float
def __init__(self, action_name, timeout=10., **kwargs):
rospy.loginfo('broadcasting action server: ' + action_name)
# won't use default auto_start=True as recommended here: https://github.com/ros/actionlib/pull/60
self._action_server = SimpleActionServer(action_name,
DetectSceneAction,
execute_cb=self._execute_cb,
auto_start=False)
self._timeout = timeout # in seconds
self._initialize(**kwargs)
self._action_server.start()
@abstractmethod
def _initialize(self, **kwargs):
pass
@abstractmethod
def _execute_cb(self, goal):
pass
@staticmethod
def is_object_on_plane(plane_msg, obj_box_msg, z_tolerance=0.05):
"""
:param z_tolerance: how much can an object be above the plane (meter)
:type plane_msg: mas_perception_msgs.msg.Plane
:type obj_box_msg: mas_perception_msgs.msg.BoundingBox
:rtype: bool
"""
obj_plane_z_diff = obj_box_msg.center.z - obj_box_msg.dimensions.z / 2 - plane_msg.plane_point.z
if obj_box_msg.center.z < plane_msg.plane_point.z or obj_plane_z_diff > z_tolerance:
return False
# assuming positive x is far away from the robot TODO(minhnh): may not work for Jenny
if obj_box_msg.center.x > plane_msg.limits.max_x:
return False
if obj_box_msg.center.y < plane_msg.limits.min_y or obj_box_msg.center.y > plane_msg.limits.max_y:
return False
return True
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 MoveitMock():
"""Mock for MoveIt which allows us to simulate errors during command execution."""
def __init__(self):
rospy.logdebug("Ctor of MoveitMock called.")
self._as_move_group = SimpleActionServer(
'move_group',
MoveGroupAction,
execute_cb=self.move_group_execute_callback,
auto_start=False)
self._as_sequence_group = SimpleActionServer(
'sequence_move_group',
MoveGroupSequenceAction,
execute_cb=self.blend_move_group_execute_callback,
auto_start=False)
self._as_move_group.start()
self._as_sequence_group.start()
# Service to signal test that node is running.
self.status_service = rospy.Service('moveit_mock_status', Empty,
self.handle_status_service)
rospy.loginfo('MoveitMock started')
def handle_status_service(self, req):
pass
def move_group_execute_callback(self, goal):
rospy.loginfo("MoveGroup execute called.")
rospy.sleep(1)
self._as_move_group.set_aborted()
return
def blend_move_group_execute_callback(self, goal):
rospy.loginfo("BlendMoveGroup execute called.")
rospy.sleep(1)
self._as_sequence_group.set_aborted()
return
class TrajectoryExecution:
def __init__(self, side_prefix):
traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server arm: ' +
side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.action_server = SimpleActionServer(motion_request_name,
JointTrajectoryAction,
execute_cb=self.move_to_joints)
self.action_server.start()
self.has_goal = False
def move_to_joints(self, traj_goal):
rospy.loginfo('Receieved a trajectory execution request.')
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
self.traj_action_client.send_goal(traj_goal)
rospy.sleep(1)
is_terminated = False
while (not is_terminated):
action_state = self.traj_action_client.get_state()
if (action_state == GoalStatus.SUCCEEDED):
self.action_server.set_succeeded()
is_terminated = True
elif (action_state == GoalStatus.ABORTED):
self.action_server.set_aborted()
is_terminated = True
elif (action_state == GoalStatus.PREEMPTED):
self.action_server.set_preempted()
is_terminated = True
rospy.loginfo('Trajectory completed.')
class DataFusionServer(object):
def __init__(self, name, topic, action_type, result_type):
self.name = name
self.result = result_type()
self.topic = topic
self.action_type = action_type
self.timeout = 5
Subscriber('mock/' + name, String, self.receive_commands)
self.server = ActionServer(self.topic, self.action_type, self.success,
False)
self.server.start()
loginfo('>>> Starting ' + self.name)
def receive_commands(self, msg):
callback, timeout = msg.data.split(':')
self.timeout = float(timeout)
self.server.execute_callback = getattr(self, callback)
logwarn('>>> ' + self.name + ': Current callback -> ' + callback)
sleep(1)
def create_random_world_model(self):
victims = [create_victim_info(i + 1) for i in range(2)]
visited = [create_victim_info(i + 1) for i in range(4)]
self.result.worldModel = create_world_model(victims, visited)
def success(self, goal):
self.create_random_world_model()
logwarn('>>> ' + self.name + ': This goal will succeed.')
sleep(self.timeout)
self.server.set_succeeded(result=self.result)
def abort(self, goal):
self.create_random_world_model()
logwarn('>>> ' + self.name + ': This goal will be aborted.')
sleep(self.timeout)
self.server.set_aborted(result=self.result)
class OSMTopologicalPlannerNode(object):
def __init__(self):
server_ip = rospy.get_param('~overpass_server_ip')
server_port = rospy.get_param('~overpass_server_port')
ref_lat = rospy.get_param('~ref_latitude')
ref_lon = rospy.get_param('~ref_longitude')
building = rospy.get_param('~building')
global_origin = [ref_lat, ref_lon]
rospy.loginfo("Server " + server_ip + ":" + str(server_port))
rospy.loginfo("Global origin: " + str(global_origin))
rospy.loginfo("Starting servers...")
self.osm_topological_planner_server = SimpleActionServer(
'/osm_topological_planner', OSMTopologicalPlannerAction, self._osm_topological_planner, False)
self.osm_topological_planner_server.start()
osm_bridge = OSMBridge(
server_ip=server_ip,
server_port=server_port,
global_origin=global_origin,
coordinate_system="cartesian",
debug=False)
path_planner = PathPlanner(osm_bridge)
path_planner.set_building(building)
self.osm_topological_planner_callback = OSMTopologicalPlannerCallback(
osm_bridge, path_planner)
rospy.loginfo(
"OSM topological planner server started. Listening for requests...")
def _osm_topological_planner(self, req):
res = self.osm_topological_planner_callback.get_safe_response(req)
if res is not None:
self.osm_topological_planner_server.set_succeeded(res)
else:
self.osm_topological_planner_server.set_aborted(res)
class 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 TakePicture(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._as = SimpleActionServer(name,
EmptyAction,
self.execute_cb,
auto_start=False)
self.app_name = rospy.get_param("~app_name",
"pose-photo/pose-photo-interactive")
self._as.start()
rospy.loginfo("... done")
def __call_service(self, srv_name, srv_type, req):
while not rospy.is_shutdown():
try:
s = rospy.ServiceProxy(srv_name, srv_type)
s.wait_for_service(timeout=1.)
except rospy.ROSException, rospy.ServiceException:
rospy.logwarn(
"Could not communicate with '%s' service. Retrying in 1 second."
% srv_name)
rospy.sleep(1.)
else:
return s(req)
class PickAndPlaceServer(object):
def __init__(self):
self.node_name = "PickAndPlaceServer"
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
# Get the object size
self.object_height = 0.1
self.object_width = 0.05
self.object_depth = 0.05
self.pick_pose = rospy.get_param('~pickup_marker_pose')
self.place_pose = rospy.get_param('~place_marker_pose')
rospy.loginfo("%s: Waiting for pickup action server...", self.node_name)
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
connected = self.pickup_ac.wait_for_server(rospy.Duration(3000))
if not connected:
rospy.logerr("%s: Could not connect to pickup action server", self.node_name)
exit()
rospy.loginfo("%s: Connected to pickup action server", self.node_name)
rospy.loginfo("%s: Waiting for place action server...", self.node_name)
self.place_ac = SimpleActionClient('/place', PlaceAction)
if not self.place_ac.wait_for_server(rospy.Duration(3000)):
rospy.logerr("%s: Could not connect to place action server", self.node_name)
exit()
rospy.loginfo("%s: Connected to place action server", self.node_name)
self.scene = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
else:
rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction,
execute_cb=self.pick_cb, auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction,
execute_cb=self.place_cb, auto_start=False)
self.place_as.start()
def pick_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.grasp_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.pick_as.set_aborted(p_res)
else:
self.pick_as.set_succeeded(p_res)
def place_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.place_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.place_as.set_aborted(p_res)
else:
self.place_as.set_succeeded(p_res)
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo(
"Waiting for object '" + object_name + "'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if 'part' is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
def grasp_object(self, object_pose):
rospy.loginfo("Removing any previous 'part' object")
self.scene.remove_attached_object("arm_tool_link")
self.scene.remove_world_object("part")
self.scene.remove_world_object("table")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
rospy.loginfo("Adding new 'part' object")
rospy.loginfo("Object pose: %s", object_pose.pose)
#Add object description in scene
self.scene.add_box("part", object_pose, (self.object_depth, self.object_width, self.object_height))
rospy.loginfo("Second%s", object_pose.pose)
table_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
table_height = object_pose.pose.position.z - self.object_width/2
table_width = 1.8
table_depth = 0.5
table_pose.pose.position.z += -(2*self.object_width)/2 -table_height/2
table_height -= 0.008 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("table", table_pose, (table_depth, table_width, table_height))
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("table")
# compute grasps
possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
goal = createPickupGoal("arm_torso", "part", object_pose, possible_grasps, self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.logdebug("Using torso result: " + str(result))
rospy.loginfo(
"Pick result: " +
str(moveit_error_dict[result.error_code.val]))
# Remove table from world
self.scene.remove_world_object("table")
return result.error_code.val
def place_object(self, object_pose):
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
possible_placings = self.sg.create_placings_from_object_pose(
object_pose)
# Try only with arm
rospy.loginfo("Trying to place using only arm")
goal = createPlaceGoal(
object_pose, possible_placings, "arm", "part", self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.loginfo(str(moveit_error_dict[result.error_code.val]))
if str(moveit_error_dict[result.error_code.val]) != "SUCCESS":
rospy.loginfo(
"Trying to place with arm and torso")
# Try with arm and torso
goal = createPlaceGoal(
object_pose, possible_placings, "arm_torso", "part", self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.logerr(str(moveit_error_dict[result.error_code.val]))
# print result
rospy.loginfo(
"Result: " +
str(moveit_error_dict[result.error_code.val]))
rospy.loginfo("Removing previous 'part' object")
self.scene.remove_world_object("part")
return result.error_code.val
class AbstractHMIServer(object):
"""
Abstract base class for a hmi client
>>> class HMIServer(AbstractHMIServer):
... def __init__(self):
... pass
... def _determine_answer(self, description, spec, choices):
... return QueryResult()
... def _set_succeeded(self, result):
... print result
>>> server = HMIServer()
>>> from hmi_msgs.msg import QueryGoal
>>> goal = QueryGoal(description='q', spec='spec', choices=[])
>>> server._execute_cb(goal)
raw_result: ''
results: []
>>> class HMIServer(AbstractHMIServer):
... def __init__(self):
... pass
>>> server = HMIServer()
Traceback (most recent call last):
...
TypeError: Can't instantiate abstract class HMIServer with abstract methods _determine_answer
"""
__metaclass__ = ABCMeta
def __init__(self, name):
self._action_name = name
self._server = SimpleActionServer(name, QueryAction,
execute_cb=self._execute_cb, auto_start=False)
self._server.start()
rospy.loginfo('HMI server started on "%s"', name)
def _execute_cb(self, goal):
# TODO: refactor this somewhere
choices = {}
for choice in goal.choices:
if choice.id in choices:
rospy.logwarn('duplicate key "%s" in answer', choice.id)
else:
choices[choice.id] = choice.values
rospy.loginfo('I got a question: %s', goal.description)
rospy.loginfo('This is the spec: %s, %s', trim_string(goal.spec), repr(choices))
try:
result = self._determine_answer(description=goal.description,
spec=goal.spec,
choices=choices,
is_preempt_requested=self._server.is_preempt_requested)
except Exception as e:
# rospy.logwarn('_determine_answer raised an exception: %s', e)
# import pdb; pdb.set_trace()
tb = traceback.format_exc()
rospy.logerr('_determine_answer raised an exception: %s' % tb)
self._server.set_aborted()
else:
# we've got a result or a cancel
if result:
self._set_succeeded(result=result.to_ros(self._action_name))
rospy.loginfo('result: %s', result)
else:
rospy.loginfo('cancelled')
self._server.set_aborted(text="Cancelled by user")
def _set_succeeded(self, result):
self._server.set_succeeded(result)
def _publish_feedback(self):
self._server.publish_feedback(QueryActionFeedback())
@abstractmethod
def _determine_answer(self, description, spec, choices, is_preempt_requested):
'''
Overwrite this method to provide custom implementations
Return the answer
Return None if nothing is heared
Raise an Exception if an error occured
'''
pass
class 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()
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 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 axServer:
def __init__(self, name):
self.fullname = name
self.jointPositions = []
self.jointVelocities = []
self.jointAccelerations = []
self.jointNamesFromConfig = []
for configJoint in configJoints:
self.jointNamesFromConfig.append(configJoint['name'])
self.jointPositions.append(0.0)
self.jointVelocities.append(0.0)
self.jointAccelerations.append(0.0)
if 'mimic_joints' in configJoint:
for mimic in configJoint['mimic_joints']:
self.jointNamesFromConfig.append(mimic['name'])
self.jointPositions.append(0.0)
self.jointVelocities.append(0.0)
self.jointAccelerations.append(0.0)
self.pointsQueue = []
self.lastTimeFromStart = 0.0
startPositions = [0.0, -0.9, -0.9, 0.0, 0.0, 0.0]
startVelocities = [0.5]*6
self.jointPositions[1] = -0.9
dynamixelChain.move_angles_sync(ids[:6], startPositions, startVelocities)
# self.joint_state_pub = rospy.Publisher('/joint_states', JointState)
self.server = SimpleActionServer(self.fullname,
FollowJointTrajectoryAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
def execute_cb(self, goal):
startTime = rospy.Time.now().to_sec()
rospy.loginfo(goal)
jNames = goal.trajectory.joint_names
self.pointsQueue = goal.trajectory.points
for a in range(len(self.pointsQueue)):
while rospy.Time.now().to_sec() - startTime < self.pointsQueue[a].time_from_start.to_sec():
rospy.sleep(0.01)
print 'sleeping...'
rospy.loginfo(rospy.Time.now().to_sec() - startTime)
if rospy.Time.now().to_sec() - startTime - self.pointsQueue[a].time_from_start.to_sec() < 0.05:
# self.executePoint(point, jNames)
if a + 1 < len(self.pointsQueue):
self.executePoint(self.pointsQueue[a], self.pointsQueue[a + 1], jNames)
else:
self.executePoint(self.pointsQueue[a], self.pointsQueue[a], jNames)
# self.publishJointStates()
self.lastTimeFromStart = rospy.Duration.from_sec(0.0)
self.pointsQueue = []
self.server.set_succeeded()
# def executePoint(self, point, jNames):
def executePoint(self, point, nextPoint, jNames):
# rospy.loginfo(point)
startTime = datetime.datetime.now()
print 'point time from start is: ' + str(point.time_from_start.to_sec())
for x in range(len(jNames)):
for y in range(len(self.jointPositions)):
if jNames[x] == self.jointNamesFromConfig[y]:
self.jointPositions[y] = point.positions[x]
self.jointVelocities[y] = point.velocities[x]
self.jointAccelerations[y] = point.accelerations[x]
# jointMover(y, point.positions[x],
# nonZeroVelocity(point.velocities[x]))
poss = matchServoVals(jNames, point.positions)
poss = matchServoVals(jNames, nextPoint.positions)
print poss
vels = nonZeroVelocities(matchServoVals(jNames, point.velocities))
print vels
accs = matchServoVals(jNames, point.accelerations)
print accs
dynamixelChain.move_angles_sync(ids[:6], poss, vels)
endTime = datetime.datetime.now()
print 'executePoint took: ' + str(endTime - startTime)
def publishJointStates(self):
# rospy.loginfo(self.jointPositions)
jState = JointState()
jState.header.stamp = rospy.Time.now()
jState.name = self.jointNamesFromConfig
jState.position = self.jointPositions
jState.velocity = self.jointVelocities
jState.effort = self.jointAccelerations
self.joint_state_pub.publish(jState)
class 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 WubbleGripperGraspController:
def __init__(self):
self.object_presence_pressure_threshold = rospy.get_param('object_presence_pressure_threshold', 200.0)
self.object_presence_opening_threshold = rospy.get_param('object_presence_opening_threshold', 0.02)
gripper_action_name = rospy.get_param('gripper_action_name', 'wubble_gripper_command_action')
self.gripper_action_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction)
#
# while not rospy.is_shutdown():
# try:
# self.gripper_action_client.wait_for_server(timeout=rospy.Duration(2.0))
# break
# except ROSException as e:
# rospy.loginfo('Waiting for %s action' % gripper_action_name)
# except:
# rospy.logerr('Unexpected error')
# raise
rospy.loginfo('Using gripper action client on topic %s' % gripper_action_name)
query_service_name = rospy.get_param('grasp_query_name', 'wubble_grasp_status')
self.query_srv = rospy.Service(query_service_name, GraspStatus, self.process_grasp_status)
posture_action_name = rospy.get_param('posture_action_name', 'wubble_gripper_grasp_action')
self.action_server = SimpleActionServer(posture_action_name, GraspHandPostureExecutionAction, self.process_grasp_action, False)
self.action_server.start()
rospy.loginfo('wubble_gripper grasp hand posture action server started on topic %s' % posture_action_name)
def process_grasp_action(self, msg):
gripper_command = WubbleGripperGoal()
if msg.goal == GraspHandPostureExecutionGoal.GRASP:
rospy.loginfo('Received GRASP request')
# if not msg.goal.grasp.position:
# msg = 'wubble gripper grasp execution: position vector empty in requested grasp'
# rospy.logerr(msg)
# self.action_server.set_aborted(text=msg)
# return
#
gripper_command.command = WubbleGripperGoal.CLOSE_GRIPPER
gripper_command.torque_limit = 0.4
gripper_command.dynamic_torque_control = True
gripper_command.pressure_upper = 1900.0
gripper_command.pressure_lower = 1800.0
elif msg.goal == GraspHandPostureExecutionGoal.PRE_GRASP:
rospy.loginfo('Received PRE_GRASP request')
gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER
gripper_command.torque_limit = 0.6
elif msg.goal == GraspHandPostureExecutionGoal.RELEASE:
rospy.loginfo('Received RELEASE request')
gripper_command.command = WubbleGripperGoal.OPEN_GRIPPER
gripper_command.torque_limit = 0.6
else:
msg = 'wubble gripper grasp execution: unknown goal code (%d)' % msg.goal
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
self.gripper_action_client.send_goal(gripper_command)
self.gripper_action_client.wait_for_result()
self.action_server.set_succeeded()
def process_grasp_status(self, msg):
result = GraspStatus()
pressure_msg = rospy.wait_for_message('/total_pressure', Float64)
opening_msg = rospy.wait_for_message('/gripper_opening', Float64)
left_pos = rospy.wait_for_message('/left_finger_controller/state', JointState).position
right_pos = rospy.wait_for_message('/right_finger_controller/state', JointState).position
if pressure_msg.data <= self.object_presence_pressure_threshold:
rospy.loginfo('Gripper grasp query false: gripper total pressure is below threshold (%.2f <= %.2f)' % (pressure_msg.data, self.object_presence_pressure_threshold))
return False
else:
if opening_msg.data <= self.object_presence_opening_threshold:
rospy.loginfo('Gripper grasp query false: gripper opening is below threshold (%.2f <= %.2f)' % (opening_msg.data, self.object_presence_opening_threshold))
return False
else:
if (left_pos > 0.85 and right_pos > 1.10) or (right_pos < -0.85 and left_pos < -1.10):
rospy.loginfo('Gripper grasp query false: gripper fingers are too off center (%.2f, %.2f)' % (left_pos, right_pos))
return False
rospy.loginfo('Gripper grasp query true: pressure is %.2f, opening is %.2f' % (pressure_msg.data, opening_msg.data))
return True
class Kill():
REFRESH_RATE = 20
def __init__(self):
self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675]
self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644]
self.v = [0] * len(self.r1)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self._action_name = 'kill'
self._tf = tf.TransformListener()
#initialize base controller
topic_name = '/base_controller/command'
self._base_publisher = rospy.Publisher(topic_name, Twist)
#Initialize the sound controller
self._sound_client = SoundClient()
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.pose = None
self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback)
#initialize this client
self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def marker_callback(self, marker):
if (self.pose is not None):
self.pose = marker.pose
def run(self, goal):
rospy.loginfo('Begin kill')
self.pose = goal.pose
#pose = self._tf.transformPose('/base_link', goal.pose)
self._sound_client.say('Halt!')
# turn to face the marker before opening arms (code repeated later)
r = rospy.Rate(self.REFRESH_RATE)
while(True):
pose = self.pose
if abs(pose.pose.position.y) > .1:
num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / ( 3 * abs(pose.pose.position.y)))
for i in range(num_move_y):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.y = 0
else:
break
# open arms
traj_goal_r = JointTrajectoryGoal()
traj_goal_r.trajectory.joint_names = self.r_joint_names
traj_goal_l = JointTrajectoryGoal()
traj_goal_l.trajectory.joint_names = self.l_joint_names
traj_goal_r.trajectory.points.append(JointTrajectoryPoint(positions=self.r1, velocities = self.v, time_from_start = rospy.Duration(2)))
self.r_traj_action_client.send_goal_and_wait(traj_goal_r, rospy.Duration(3))
traj_goal_l.trajectory.points.append(JointTrajectoryPoint(positions=self.l1, velocities = self.v, time_from_start = rospy.Duration(2)))
self.l_traj_action_client.send_goal_and_wait(traj_goal_l, rospy.Duration(3))
traj_goal_r.trajectory.points[0].positions = self.r2
self.r_traj_action_client.send_goal(traj_goal_r)
traj_goal_l.trajectory.points[0].positions = self.l2
self.l_traj_action_client.send_goal(traj_goal_l)
self._sound_client.say('You have the right to remain silent.')
# Keep a local copy because it will update
#pose = self.pose
#num_move_x = int((pose.pose.position.x - 0.3) * 10 / .1) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
#twist_msg = Twist()
#twist_msg.linear = Vector3(.1, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, 0.0)
#for i in range(num_move_x):
# self._base_publisher.publish(twist_msg)
# r.sleep()
# track the marker as much as we can
while True:
pose = self.pose
# too far away
if abs(pose.pose.position.x > 1.5):
rospy.loginfo('Target out of range: ' + str(pose.pose.position.x))
self._as.set_aborted()
return;
# too far to the side -> rotate
elif abs(pose.pose.position.y) > .1:
num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (3 * abs(pose.pose.position.y)))
for i in range(num_move_y):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.y = 0
#twist_msg = Twist()
#twist_msg.linear = Vector3(0.0, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (5 * abs(pose.pose.position.y)))
#self._base_publisher.publish(twist_msg)
# now we are going to move in for the kill, but only until .5 meters away (don't want to run suspect over)
elif pose.pose.position.x > .5:
num_move_x = int((pose.pose.position.x - 0.3) * self.REFRESH_RATE / .1) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(.1, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
for i in range(num_move_x):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.x = 0
#twist_msg = Twist()
#twist_msg.linear = Vector3(.1, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, 0.0)
#self._base_publisher.publish(twist_msg)
# susupect is within hugging range!!!
else:
break
r.sleep()
# after exiting the loop, we should be ready to capture -> send close arms goal
self._sound_client.say("Anything you say do can and will be used against you in a court of law.")
self.l_traj_action_client.wait_for_result(rospy.Duration(3))
self.r_traj_action_client.wait_for_result(rospy.Duration(3))
traj_goal_r.trajectory.points[0].positions = self.r3
self.r_traj_action_client.send_goal(traj_goal_r)
traj_goal_l.trajectory.points[0].positions = self.l3
self.l_traj_action_client.send_goal(traj_goal_l)
self.l_traj_action_client.wait_for_result(rospy.Duration(3))
self.r_traj_action_client.wait_for_result(rospy.Duration(3))
rospy.loginfo('End kill')
rospy.sleep(20)
self._as.set_succeeded()
class OnlineBagger(object):
BAG_TOPIC = '/online_bagger/bag'
def __init__(self):
"""
Make dictionary of dequeues.
Subscribe to set of topics defined by the yaml file in directory
Stream topics up to a given stream time, dump oldest messages when limit is reached
Set up service to bag n seconds of data default to all of available data
"""
self.successful_subscription_count = 0 # successful subscriptions
self.iteration_count = 0 # number of iterations
self.streaming = True
self.get_params()
if len(self.subscriber_list) == 0:
rospy.logwarn('No topics selected to subscribe to. Closing.')
rospy.signal_shutdown('No topics to subscribe to')
return
self.make_dicts()
self._action_server = SimpleActionServer(OnlineBagger.BAG_TOPIC, BagOnlineAction,
execute_cb=self.start_bagging, auto_start=False)
self.subscribe_loop()
rospy.loginfo('Remaining Failed Topics: {}\n'.format(
self.get_subscriber_list(False)))
self._action_server.start()
def get_subscriber_list(self, status):
"""
Get string of all topics, if their subscribe status matches the input (True / False)
Outputs each topics: time_buffer(float in seconds), subscribe_statue(bool), topic(string)
"""
sub_list = ''
for topic in self.subscriber_list.keys():
if self.subscriber_list[topic][1] == status:
sub_list = sub_list + \
'\n{:13}, {}'.format(self.subscriber_list[topic], topic)
return sub_list
def get_params(self):
"""
Retrieve parameters from param server.
"""
self.dir = rospy.get_param('~bag_package_path', default=None)
# Handle bag directory for MIL bag script
if self.dir is None and 'BAG_DIR' in os.environ:
self.dir = os.environ['BAG_DIR']
self.stream_time = rospy.get_param(
'~stream_time', default=30) # seconds
self.resubscribe_period = rospy.get_param(
'~resubscribe_period', default=3.0) # seconds
self.dated_folder = rospy.get_param(
'~dated_folder', default=True) # bool
self.subscriber_list = {}
topics_param = rospy.get_param('~topics', default=[])
# Add topics from rosparam to subscribe list
for topic in topics_param:
time = topic[1] if len(topic) == 2 else self.stream_time
self.subscriber_list[topic[0]] = (time, False)
def add_unique_topic(topic):
if topic not in self.subscriber_list:
self.subscriber_list[topic] = (self.stream_time, False)
def add_env_var(var):
for topic in var.split():
add_unique_topic(topic)
# Add topics from MIL bag script environment variables
if 'BAG_ALWAYS' in os.environ:
add_env_var(os.environ['BAG_ALWAYS'])
for key in os.environ.keys():
if key[0:4] == 'bag_':
add_env_var(os.environ[key])
rospy.loginfo(
'Default stream_time: {} seconds'.format(self.stream_time))
rospy.loginfo('Bag Directory: {}'.format(self.dir))
def make_dicts(self):
"""
Make dictionary with sliceable deques() that will be filled with messages and time stamps.
Subscriber list contains all of the topics, their stream time and their subscription status:
A True status for a given topic corresponds to a successful subscription
A False status indicates a failed subscription.
Stream time for an individual topic is specified in seconds.
For Example:
self.subscriber_list[0:1] = [['/odom', 300 ,False], ['/absodom', 300, True]]
Indicates that '/odom' has not been subscribed to, but '/absodom' has been subscribed to
self.topic_messages is a dictionary of deques containing a list of tuples.
Dictionary Keys contain topic names
Each value for each topic contains a deque
Each deque contains a list of tuples
Each tuple contains a message and its associated time stamp
For example:
'/odom' is a potential topic name
self.topic_message['/odom'] is a deque
self.topic_message['/odom'][0] is the oldest message available in the deque
and its time stamp if available. It is a tuple with each element: (time_stamp, msg)
self.topic_message['/odom'][0][0] is the time stamp for the oldest message
self.topic_message['/odom'][0][1] is the message associated with the oldest topic
"""
self.topic_messages = {}
class SliceableDeque(deque):
def __getitem__(self, index):
if isinstance(index, slice):
return type(self)(itertools.islice(self, index.start,
index.stop, index.step))
return deque.__getitem__(self, index)
for topic in self.subscriber_list:
self.topic_messages[topic] = SliceableDeque(deque())
rospy.loginfo('Initial subscriber_list: {}'.format(
self.get_subscriber_list(False)))
def subscribe_loop(self):
"""
Continue to subscribe until at least one topic is successful,
then break out of loop and be called in the callback function to prevent the function
from locking up.
"""
self.resubscriber = None
i = 0
# if self.successful_subscription_count == 0 and not
# rospy.is_shutdown():
while self.successful_subscription_count == 0 and not rospy.is_shutdown():
self.subscribe()
rospy.sleep(0.1)
i = i + 1
if i % 1000 == 0:
rospy.logdebug('still subscribing!')
rospy.loginfo("Subscribed to {} of {} topics, will try again every {} seconds".format(
self.successful_subscription_count, len(self.subscriber_list), self.resubscribe_period))
self.resubscriber = rospy.Timer(rospy.Duration(
self.resubscribe_period), self.subscribe)
def subscribe(self, time_info=None):
"""
Subscribe to the topics defined in the yaml configuration file
Function checks subscription status True/False of each topic
if True: topic has already been sucessfully subscribed to
if False: topic still needs to be subscribed to and
subscriber will be run.
Each element in self.subscriber list is a list [topic, Bool]
where the Bool tells the current status of the subscriber (sucess/failure).
Return number of topics that failed subscription
"""
if self.successful_subscription_count == len(self.subscriber_list):
if self.resubscriber is not None:
self.resubscriber.shutdown()
rospy.loginfo(
'All topics subscribed too! Shutting down resubscriber')
for topic, (time, subscribed) in self.subscriber_list.items():
if not subscribed:
msg_class = rostopic.get_topic_class(topic)
if msg_class[1] is not None:
self.successful_subscription_count += 1
rospy.Subscriber(topic, msg_class[0],
lambda msg, _topic=topic: self.bagger_callback(msg, _topic))
self.subscriber_list[topic] = (time, True)
def get_topic_duration(self, topic):
"""
Return current time duration of topic
"""
return self.topic_messages[topic][-1][0] - self.topic_messages[topic][0][0]
def get_header_time(self, msg):
"""
Retrieve header time if available
"""
if hasattr(msg, 'header'):
return msg.header.stamp
else:
return rospy.get_rostime()
def get_time_index(self, topic, requested_seconds):
"""
Return the index for the time index for a topic at 'n' seconds from the end of the dequeue
For example, to bag the last 10 seconds of data, the index for 10 seconds back from the most
recent message can be obtained with this function.
The number of requested seconds should be the number of seoncds desired from
the end of deque. (ie. requested_seconds = 10 )
If the desired time length of the bag is greater than the available messages it will output a
message and return how ever many seconds of data are avaiable at the moment.
Seconds is of a number type (not a rospy.Time type) (ie. int, float)
"""
topic_duration = self.get_topic_duration(topic).to_sec()
if topic_duration == 0:
return 0
ratio = requested_seconds / topic_duration
index = int(self.get_topic_message_count(topic) * (1 - min(ratio, 1)))
return index
def bagger_callback(self, msg, topic):
"""
Streaming callback function, stops streaming during bagging process
also pops off msgs from dequeue if stream size is greater than specified stream_time
Stream, callback function does nothing if streaming is not active
"""
if not self.streaming:
return
self.iteration_count = self.iteration_count + 1
time = self.get_header_time(msg)
self.topic_messages[topic].append((time, msg))
time_diff = self.get_topic_duration(topic)
# verify streaming is popping off and recording topics
if self.iteration_count % 100 == 0:
rospy.logdebug("{} has {} messages spanning {} seconds".format(
topic, self.get_topic_message_count(topic), round(time_diff.to_sec(), 2)))
while time_diff > rospy.Duration(self.subscriber_list[topic][0]) and not rospy.is_shutdown():
self.topic_messages[topic].popleft()
time_diff = self.get_topic_duration(topic)
def get_topic_message_count(self, topic):
"""
Return number of messages available in a topic
"""
return len(self.topic_messages[topic])
def get_total_message_count(self):
"""
Returns total number of messages across all topics
"""
total_message_count = 0
for topic in self.topic_messages.keys():
total_message_count = total_message_count + \
self.get_topic_message_count(topic)
return total_message_count
def _get_default_filename(self):
return str(datetime.date.today()) + '-' + str(datetime.datetime.now().time())[0:8]
def get_bag_name(self, filename=''):
"""
Create ros bag save directory
If no bag name is provided, the current date/time is used as default.
"""
# If directory param is not set, default to $HOME/bags/<date>
default_dir = self.dir
if default_dir is None:
default_dir = os.path.join(os.environ['HOME'], 'bags')
# if dated folder param is set to True, append current date to
# directory
if self.dated_folder is True:
default_dir = os.path.join(default_dir, str(datetime.date.today()))
# Split filename from directory
bag_dir, bag_name = os.path.split(filename)
bag_dir = os.path.join(default_dir, bag_dir)
if not os.path.exists(bag_dir):
os.makedirs(bag_dir)
# Create default filename if only directory specified
if bag_name == '':
bag_name = self._get_default_filename()
# Make sure filename ends in .bag, add otherwise
if bag_name[-4:] != '.bag':
bag_name = bag_name + '.bag'
return os.path.join(bag_dir, bag_name)
def start_bagging(self, req):
"""
Dump all data in dictionary to bags, temporarily stops streaming
during the bagging process, resumes streaming when over.
If bagging is already false because of an active call to this service
"""
result = BagOnlineResult()
if self.streaming is False:
result.status = 'Bag Request came in while bagging, priority given to prior request'
result.success = False
self._action_server.set_aborted(result)
return
bag = None
try:
self.streaming = False
result.filename = self.get_bag_name(req.bag_name)
bag = rosbag.Bag(result.filename, 'w')
requested_seconds = req.bag_time
selected_topics = req.topics.split()
feedback = BagOnlineFeedback()
total_messages = 0
bag_topics = {}
for topic, (time, subscribed) in self.subscriber_list.iteritems():
if not subscribed:
continue
# Exclude topics that aren't in topics service argument
# If topics argument is empty string, include all topics
if len(selected_topics) > 0 and topic not in selected_topics:
continue
if len(self.topic_messages[topic]) == 0:
continue
if req.bag_time == 0:
index = 0
else:
index = self.get_time_index(topic, requested_seconds)
total_messages += len(self.topic_messages[topic][index:])
bag_topics[topic] = index
if total_messages == 0:
result.success = False
result.status = 'no messages'
self._action_server.set_aborted(result)
self.streaming = True
bag.close()
return
self._action_server.publish_feedback(feedback)
msg_inc = 0
for topic, index in bag_topics.iteritems():
for msgs in self.topic_messages[topic][index:]:
bag.write(topic, msgs[1], t=msgs[0])
if msg_inc % 50 == 0: # send feedback every 50 messages
feedback.progress = float(msg_inc) / total_messages
self._action_server.publish_feedback(feedback)
msg_inc += 1
# empty deque when done writing to bag
self.topic_messages[topic].clear()
feedback.progress = 1.0
self._action_server.publish_feedback(feedback)
bag.close()
except Exception as e:
result.success = False
result.status = 'Exception while writing bag: ' + str(e)
self._action_server.set_aborted(result)
self.streaming = True
if bag is not None:
bag.close()
return
rospy.loginfo('Bag written to {}'.format(result.filename))
result.success = True
self._action_server.set_succeeded(result)
self.streaming = True
class Robot001Manager(object):
def __init__(self, ip_robot):
self.ip_robot = ip_robot
# pan tilt
self.joint_states = [None, None]
self.js_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=5)
self.as_ = SimpleActionServer('/robot_controller',
JointTrajectoryAction,
auto_start=False,
execute_cb=self.goal_cb)
self.as_.start()
self.go_to_position(pan=0.0, tilt=0.0)
rospy.Timer(rospy.Duration(0.02), self.js_pub_cb, oneshot=False)
rospy.loginfo("We are started!")
def js_pub_cb(self, params):
js = JointState()
js.header.stamp = rospy.Time.now()
js.name = ['pan_joint', 'tilt_joint']
if self.joint_states[0] is None:
return
js.position = self.joint_states
self.js_pub.publish(js)
def goal_cb(self, goal):
#goal = JointTrajectoryGoal()
rospy.loginfo("Goal: " + str(goal))
pan_idx = goal.trajectory.joint_names.index('pan_joint')
tilt_idx = goal.trajectory.joint_names.index('tilt_joint')
for p in goal.trajectory.points:
pan_pos = p.positions[pan_idx]
tilt_pos = p.positions[tilt_idx]
# self.go_to_position(pan=pan_pos, tilt=tilt_pos)
self.as_.set_succeeded(JointTrajectoryResult())
def go_to_position(self, pan, tilt):
if pan >= 0.0:
self.go_to_right(pan)
elif pan < 0.0:
self.go_to_left(pan)
if tilt >= 0.0:
self.go_to_up(tilt)
elif tilt < 0.0:
self.go_to_down(tilt)
def update_joints(self, ret_text):
result_d = re.search("Down=((\d+)\.(\d+))", ret_text)
result_r = re.search("Right=((\d+)\.(\d+))", ret_text)
try:
pan = float(result_r.groups()[0])
tilt = float(result_d.groups()[0])
self.joint_states = [radians(pan), radians(tilt)]
except AttributeError as e:
rospy.logwarn("Attribute error when parsing: " + str(e))
#self.go_to_position(pan=0.0, tilt=0.0)
def go_to_left(self, qtty):
if qtty < 0.0:
qtty = qtty * -1.0
r = requests.get('http://' + self.ip_robot +
'?l=' + str(int(qtty)))
self.update_joints(r.text)
def go_to_right(self, qtty):
if qtty < 0.0:
qtty = qtty * -1.0
r = requests.get('http://' + self.ip_robot +
'?r=' + str(int(qtty)))
self.update_joints(r.text)
def go_to_up(self, qtty):
if qtty < 0.0:
qtty = qtty * -1.0
r = requests.get('http://' + self.ip_robot +
'?u=' + str(int(qtty)))
self.update_joints(r.text)
def go_to_down(self, qtty):
if qtty < 0.0:
qtty = qtty * -1.0
r = requests.get('http://' + self.ip_robot +
'?d=' + str(int(qtty)))
self.update_joints(r.text)
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 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 PathExecutor:
def __init__(self):
self._result = ExecutePathResult()
self._feedback = ExecutePathFeedback()
self._action_name = "/path_executor/execute_path"
self._as = SimpleActionServer(
self._action_name, ExecutePathAction, execute_cb=self.execute_cb, auto_start=False
)
self._as.start()
rospy.loginfo("Server start")
# initialise current path publisher
self._path_publisher = rospy.Publisher("/path_executor/current_path", Path)
# initialise Bug2 client
if rospy.get_param("~use_obstacle_avoidance"):
MOVE_SERVER = "/bug2/move_to"
else:
MOVE_SERVER = "/motion_controller/move_to"
print MOVE_SERVER
self._move_client = SimpleActionClient(MOVE_SERVER, MoveToAction)
print "Connecting to bug server...", MOVE_SERVER
self._move_client.wait_for_server()
def move_to_next_node(self):
print "moveto next node"
if len(self._poses_chain):
bug_goal = MoveToGoal()
bug_goal.target_pose = self._poses_chain.pop(0)
self._move_client.send_goal(bug_goal, done_cb=self.move_to_done_cb, feedback_cb=self.move_to_feedback_cb)
else:
return
# self._result.visited = [True]
# self._as.set_succeeded(self._result)
def execute_cb(self, goal):
# rospy.loginfo('CALLBACK')
# self._feedback.reached = True
# self._as.publish_feedback(self._feedback)
rospy.loginfo("----- ----- -----")
rospy.loginfo("Path acquired")
r = rospy.Rate(10)
self.MOVING = True
self._poses_chain = goal.path.poses
self.move_to_next_node()
while self.MOVING:
r.sleep()
"""
if self._as.is_preempt_requested():
rospy.loginfo()
self._poses_chain = []
self._as.set_preempted()
"""
# self._path_publisher.publish(goal.path)
"""
Callbacks of MOVETO client
"""
def move_to_done_cb(self, state, result):
print "MOVETO DONE CALLBACK"
# for i in result:
# print i
# rospy.loginfo(type(result))
# print dir(state)
# print type(state)
print "STATE", state
self.move_to_next_node()
def move_to_feedback_cb(self, feedback):
# print 'MOVETO FEEDBACK CALLBACK'
print self._move_client.get_goal_status_text()
# print dir(feedback)
print type(feedback)
# rospy.loginfo('FEEDBACK: '%feedback.status)
pass
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 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 axServer:
def __init__(self, name, configJoints):
self.fullname = name
self.jointNames = []
for configJoint in configJoints:
self.jointNames.append(configJoint["name"])
self.jointNames = self.jointNames[:4]
self.failureState = False
self.goalPositions = [0.0, -0.9, 0.0, 0.0]
self.goalVelocities = [0.5] * 4
self.move_chain()
# todo: add method for checking arm's actual positions
self.actualPositions = [0.0, -0.9, 0.0, 0.0]
self.server = SimpleActionServer(
self.fullname, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False
)
self.server.start()
def move_chain(self):
# moves the Dynamixel Chain to the object's goalPositions
# and goalVelocities
orderedIDs = [[1], [2, 3], [4, 5], [6]]
commandIDs = []
commandPositions = []
commandVelocities = []
for i in range(len(self.goalPositions)):
for orderedID in orderedIDs[i]:
commandIDs.append(orderedID)
commandPositions.append(self.goalPositions[i])
commandVelocities.append(self.goalVelocities[i])
dynamixelChain.move_angles_sync(commandIDs, commandPositions, commandVelocities)
def execute_cb(self, goal):
startTime = rospy.Time.now().to_sec()
rospy.loginfo(goal)
jNames = goal.trajectory.joint_names
pointsQueue = goal.trajectory.points
for a in range(len(pointsQueue)):
pointTime = pointsQueue[a].time_from_start.to_sec()
timeSlept = 0.0
while rospy.Time.now().to_sec() - startTime < pointTime:
rospy.sleep(0.01)
timeSlept += 0.01
print "slept " + str(timeSlept) + " seconds between points"
rospy.loginfo(rospy.Time.now().to_sec() - startTime)
if a == 0:
self.executePoint(pointsQueue[a], pointsQueue[a], jNames)
else:
self.executePoint(pointsQueue[a - 1], pointsQueue[a], jNames)
self.server.set_succeeded()
def executePoint(self, point1, point2, jNames):
# moves arm to point2's positions at the greater
# absolute Velocites of the two points
startTime = datetime.datetime.now()
for x in range(len(jNames)):
if jNames[x] in self.jointNames:
y = self.jointNames.index(jNames[x])
self.goalPositions[y] = point2.positions[x]
if abs(point1.velocities[x]) > abs(point2.velocities[x]):
self.goalVelocities[y] = abs(point1.velocities[x])
else:
self.goalVelocities[y] = abs(point2.velocities[x])
else:
print "we have a problem: joint names are not lining up"
self.move_chain()
endTime = datetime.datetime.now()
print "executePoint took: " + str(endTime - startTime)
def checkFailureState(self):
if self.failureState:
print "I am currently in a failure state."
class MotionService(object):
def __init__(self):
rospy.init_node('motion_service')
# Load config
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
# Create publisher for first target
if len(config_loader.targets) > 0:
self._motion_publisher = MotionPublisher(
config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix)
else:
rospy.logerr('Robot config needs to contain at least one valid target.')
self._motion_data = MotionData(config_loader.robot_config)
# Subscriber to start motions via topic
self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb)
# Create action server
self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False)
self._action_server.register_goal_callback(self._execute_motion_goal)
self._action_server.register_preempt_callback(self._preempt_motion_goal)
self._preempted = False
def _execute_motion_goal(self):
goal = self._action_server.accept_new_goal()
# Check if motion exists
if goal.motion_name not in self._motion_data:
result = ExecuteMotionResult()
result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN]
result.error_string = "The requested motion is unknown."
self._action_server.set_aborted(result)
return
# check if a valid time_factor is set, otherwise default to 1.0
if goal.time_factor <= 0.0:
goal.time_factor = 1.0
self._preempted = False
self._motion_publisher.publish_motion(self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished)
print '[MotionService] New action goal received.'
def _preempt_motion_goal(self):
self._motion_publisher.stop_motion()
print '[MotionService] Preempting goal.'
self._preempted = True
def _trajectory_finished(self, error_codes, error_groups):
result = ExecuteMotionResult()
result.error_code = error_codes
error_string = ""
for error_code, error_group in zip(error_codes, error_groups):
error_string += error_group + ': ' + str(error_code) + '\n'
result.error_string = error_string
if self._preempted:
self._action_server.set_preempted(result)
else:
self._action_server.set_succeeded(result)
print '[MotionService] Trajectory finished with error code: \n', error_string
def _execute_motion(self, request):
response = ExecuteMotionResponse()
# check if a motion with this name exists
if request.motion_name not in self._motion_data:
print '[MotionService] Unknown motion name: "%s"' % request.motion_name
response.ok = False
response.finish_time.data = rospy.Time.now()
return response
# check if a valid time_factor is set, otherwise default to 1.0
if request.time_factor <= 0.0:
request.time_factor = 1.0
# find the duration of the requested motion
motion_duration = 0.0
for poses in self._motion_data[request.motion_name].values():
if len(poses) > 0:
endtime = poses[-1]['starttime'] + poses[-1]['duration']
motion_duration = max(motion_duration, endtime)
motion_duration *= request.time_factor
# execute motion
print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (request.motion_name, request.time_factor, motion_duration)
self._motion_publisher.publish_motion(self._motion_data[request.motion_name], request.time_factor)
# reply with ok and the finish_time of this motion
response.ok = True
response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(motion_duration)
return response
def start_server(self):
rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion, self._execute_motion)
self._action_server.start()
print '[MotionService] Waiting for calls...'
rospy.spin()
def motion_command_request_cb(self, command):
print "[MotionService] Initiate motion command via topic ..."
request = ExecuteMotion()
request.motion_name = command.motion_name
request.time_factor = command.time_factor
self._execute_motion(request)
print "[MotionService] Motion command complete"