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 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 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 TestServer:
def __init__(self,name):
self._sas = SimpleActionServer(name,
TestAction,
execute_cb=self.execute_cb)
def execute_cb(self, msg):
if msg.goal == 0:
self._sas.set_succeeded()
elif msg.goal == 1:
self._sas.set_aborted()
class MockActionServer(object):
""" MockActionServer base class """
def __init__(self, name, topic, action_type):
""" Creating a custom mock action server."""
self._topic = topic
self._name = name
self._action_type = action_type
self.timeout = 1
self.action_result = None
self.prefered_callback = ' '
Subscriber('mock/' + name, String, self.receive_commands)
Subscriber('mock/gui_result', Bool, self.set_gui_result)
self._server = ActionServer(self._topic, self._action_type,
self.decide, False)
self._server.start()
loginfo('>>> Starting ' + self._name)
def receive_commands(self, msg):
""" Decides the result of the next call. """
callback, timeout = msg.data.split(':')
# self.timeout = float(timeout)
self.prefered_callback = callback
sleep(1)
def decide(self, goal):
""" Deciding outcome of goal """
logwarn('Deciding callback...')
sleep(self.timeout)
if(self.prefered_callback == 'success'):
logwarn('>>> ' + self._name + ': This goal will succeed.')
self._server.set_succeeded()
elif(self.prefered_callback == 'abort'):
logwarn('>>> ' + self._name + ': This goal will be aborted.')
self._server.set_aborted()
elif(self.prefered_callback == 'preempt'):
logwarn('>>> ' + self._name + ': This goal will be preempted.')
self._server.set_preempted()
else:
logwarn('wtf?')
def set_gui_result(self, msg):
""" Sets the result of the goal. """
self.action_result = ValidateVictimGUIResult()
logwarn('>>> The gui response will be: ' + str(msg.data))
self.action_result.victimValid = msg.data
class 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 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 MockEndEffectorPlanner():
def __init__(self, end_effector_planner_topic):
self.moves_end_effector = False
self.move_end_effector_succeeded = False
self.reply = False
self.command = 0
self.point_of_interest = ""
self.center_point = ""
self.end_effector_planner_as_ = SimpleActionServer(
end_effector_planner_topic,
MoveEndEffectorAction,
execute_cb=self.end_effector_planner_cb,
auto_start=False)
self.end_effector_planner_as_.start()
def __del__(self):
self.end_effector_planner_as_.__del__()
def end_effector_planner_cb(self, goal):
rospy.loginfo('end_effector_planner_cb')
self.moves_end_effector = True
self.command = goal.command
self.point_of_interest = goal.point_of_interest
self.center_point = goal.center_point
while not self.reply:
rospy.sleep(1.)
if self.end_effector_planner_as_.is_preempt_requested():
self.end_effector_planner_as_.set_preempted(MoveEndEffectorResult())
self.moves_end_effector = False
return None
else:
self.reply = False
result = MoveEndEffectorResult()
if self.move_end_effector_succeeded:
self.moves_end_effector = False
self.end_effector_planner_as_.set_succeeded(result)
else:
self.moves_end_effector = False
self.end_effector_planner_as_.set_aborted(result)
class AveragingSVR(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
execute_cb=self.execute_cb,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def preempt_cb(self):
rospy.loginfo('preempt callback')
self._action.set_preempted(text='message for preempt')
def execute_cb(self, goal):
rospy.loginfo('execute callback: %s' % (goal))
feedback = AveragingAction().action_feedback.feedback
result = AveragingAction().action_result.result
## execute loop
rate = rospy.Rate(1 / (0.01 + 0.99 * random.random()))
samples = []
for i in range(goal.samples):
sample = random.random()
samples.append(sample)
feedback.sample = i
feedback.data = sample
feedback.mean = sum(samples) / len(samples)
feedback.std_dev = self.std_dev(samples)
self._action.publish_feedback(feedback)
rate.sleep()
if (not self._action.is_active()):
rospy.loginfo('not active')
return
## sending result
result.mean = sum(samples) / len(samples)
result.std_dev = self.std_dev(samples)
rospy.loginfo('result: %s' % (result))
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result, text='message for aborted')
class 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 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 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 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 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 HokuyoLaserActionServer():
def __init__(self):
# Initialize constants
self.error_threshold = 0.0175 # Report success if error reaches below threshold
self.signal = 1
# Initialize new node
rospy.init_node(NAME, anonymous=True)
controller_name = rospy.get_param('~controller')
# Initialize publisher & subscriber for tilt
self.laser_tilt = JointControllerState()
self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64)
self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal)
self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True)
rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state)
rospy.wait_for_message(controller_name + '/state', JointControllerState)
# Initialize tilt action server
self.result = HokuyoLaserTiltResult()
self.feedback = HokuyoLaserTiltFeedback()
self.feedback.tilt_position = self.laser_tilt.process_value
self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback)
rospy.loginfo("%s: Ready to accept goals", NAME)
def process_tilt_state(self, tilt_data):
self.laser_tilt = tilt_data
def reset_tilt_position(self, offset=0.0):
self.laser_tilt_pub.publish(offset)
rospy.sleep(0.5)
def execute_callback(self, goal):
r = rospy.Rate(100)
self.joint_speed_srv(2.0)
self.reset_tilt_position(goal.offset)
delta = goal.amplitude - goal.offset
target_speed = delta / goal.duration
timeout_threshold = rospy.Duration(5.0) + rospy.Duration.from_sec(goal.duration)
self.joint_speed_srv(target_speed)
print "delta = %f, target_speed = %f" % (delta, target_speed)
self.result.success = True
self.result.tilt_position = self.laser_tilt.process_value
rospy.loginfo("%s: Executing laser tilt %s time(s)", NAME, goal.tilt_cycles)
# Tilt laser goal.tilt_cycles amount of times.
for i in range(1, goal.tilt_cycles + 1):
self.feedback.progress = i
# Issue 2 commands for each cycle
for j in range(2):
if j % 2 == 0:
target_tilt = goal.offset + goal.amplitude # Upper tilt limit
self.signal = 0
else:
target_tilt = goal.offset # Lower tilt limit
self.signal = 1
# Publish target command to controller
self.laser_tilt_pub.publish(target_tilt)
start_time = rospy.Time.now()
current_time = start_time
while abs(target_tilt - self.laser_tilt.process_value) > self.error_threshold:
#delta = abs(target_tilt - self.laser_tilt.process_value)
#time_left = goal.duration - (rospy.Time.now() - start_time).to_sec()
#target_speed = delta / time_left
#self.joint_speed_srv(target_speed)
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
return
# Publish current head position as feedback
self.feedback.tilt_position = self.laser_tilt.process_value
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > timeout_threshold):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
return
r.sleep()
signal = LaserScannerSignal()
signal.header.stamp = current_time
signal.signal = self.signal
self.laser_signal_pub.publish(signal)
#rospy.sleep(0.5)
if self.result.success:
rospy.loginfo("%s: Goal Completed", NAME)
self.result.tilt_position = self.laser_tilt.process_value
self.server.set_succeeded(self.result)
class SocialGaze:
'''Handles where to point robot's head'''
def __init__(self):
self._default_focus_point = Point(1, 0, 1.05)
self._down_focus_point = Point(0.5, 0, 0.5)
self._target_focus_point = Point(1, 0, 1.05)
self._current_focus_point = Point(1, 0, 1.05)
self._current_gaze_action = GazeGoal.LOOK_FORWARD
self._prev_gaze_action = self._current_gaze_action
self._prev_target_focus_point = array(self._default_focus_point)
# Some constants
self._no_interrupt = [GazeGoal.NOD,
GazeGoal.SHAKE, GazeGoal.LOOK_DOWN]
self._nod_positions = [Point(1, 0, 0.70), Point(1, 0, 1.20)]
self._shake_positions = [Point(1, 0.2, 1.05), Point(1, -0.2, 1.05)]
self._n_nods = 5
self._n_shakes = 5
self._nod_counter = 5
self._shake_counter = 5
self._face_pos = None
self._glance_counter = 0
self.gaze_goal_strs = {
GazeGoal.LOOK_FORWARD: 'LOOK_FORWARD',
GazeGoal.FOLLOW_EE: 'FOLLOW_EE',
GazeGoal.GLANCE_EE: 'GLANCE_EE',
GazeGoal.NOD: 'NOD',
GazeGoal.SHAKE: 'SHAKE',
GazeGoal.FOLLOW_FACE: 'FOLLOW_FACE',
GazeGoal.LOOK_AT_POINT: 'LOOK_AT_POINT',
GazeGoal.LOOK_DOWN: 'LOOK_DOWN',
GazeGoal.NONE: 'NONE',
}
## Action client for sending commands to the head.
self._head_action_client = SimpleActionClient(
'/head_controller/point_head', PointHeadAction)
self._head_action_client.wait_for_server()
rospy.loginfo('Head action client has responded.')
self._head_goal = PointHeadGoal()
self._head_goal.target.header.frame_id = 'base_link'
self._head_goal.min_duration = rospy.Duration(1.0)
self._head_goal.target.point = Point(1, 0, 1)
## Service client for arm states
self._tf_listener = TransformListener()
## Server for gaze requested gaze actions
self._gaze_action_server = SimpleActionServer(
'gaze_action', GazeAction, self._execute_gaze_action, False)
self._gaze_action_server.start()
self._is_action_complete = True
rospy.Service('get_current_gaze_goal', GetGazeGoal,
self._get_gaze_goal)
# ##################################################################
# Instance methods: Public (API)
# ##################################################################
def update(self):
'''Update goal for head movement'''
is_action_possibly_complete = True
if self._current_gaze_action == GazeGoal.FOLLOW_EE:
self._target_focus_point = self._get_ee_pos()
elif self._current_gaze_action == GazeGoal.NOD:
self._target_focus_point = self._get_next_nod_point(
self._current_focus_point, self._target_focus_point)
self._head_goal.min_duration = rospy.Duration(0.5)
is_action_possibly_complete = False
elif self._current_gaze_action == GazeGoal.SHAKE:
self._target_focus_point = self._get_next_shake_point(
self._current_focus_point, self._target_focus_point)
self._head_goal.min_duration = rospy.Duration(0.5)
is_action_possibly_complete = False
elif self._current_gaze_action == GazeGoal.GLANCE_EE:
self._target_focus_point = self._get_next_glance_point(
self._current_focus_point, self._target_focus_point)
is_action_possibly_complete = False
self._current_focus_point = SocialGaze._filter_look_at_position(
self._current_focus_point, self._target_focus_point)
if self._current_gaze_action is GazeGoal.NONE:
pass
elif (SocialGaze._is_the_same(
SocialGaze._point2array(self._head_goal.target.point),
SocialGaze._point2array(self._target_focus_point))):
if is_action_possibly_complete:
head_state = self._head_action_client.get_state()
if head_state == GoalStatus.SUCCEEDED:
self._is_action_complete = True
else:
self._head_goal.target.point.x = self._current_focus_point.x
self._head_goal.target.point.y = self._current_focus_point.y
self._head_goal.target.point.z = self._current_focus_point.z
self._head_action_client.send_goal(self._head_goal)
time.sleep(0.02)
# ##################################################################
# Static methods: Internal ("private")
# ##################################################################
@staticmethod
def _is_the_same(current, target):
'''Get 3DOF pose of end effector
Args:
current (array): (x, y, z) of point
target (array): (x, y, z) of point
Returns:
bool
'''
diff = target - current
dist = linalg.norm(diff)
return dist < 0.003
@staticmethod
def _point2array(p):
'''Make Point msg into array
Args:
p (Point)
Returns:
array
'''
return array((p.x, p.y, p.z))
@staticmethod
def _array2point(a):
'''Make array into Point msg
Args:
a (array)
Returns:
Point
'''
return Point(a[0], a[1], a[2])
@staticmethod
def _filter_look_at_position(current, target):
'''If head goal is too far away, returns an intermediate position
to limit speed
Args:
current (Point): current head goal
target (Point): new head goal
Returns:
Point
'''
speed = 0.02
diff = (SocialGaze._point2array(target) -
SocialGaze._point2array(current))
dist = linalg.norm(diff)
if dist > speed:
step = dist / speed
return SocialGaze._array2point(SocialGaze._point2array(current) +
diff / step)
else:
return target
# ##################################################################
# Instance methods: Internal ("private")
# ##################################################################
def _get_gaze_goal(self, req):
'''Return current gaze goal
Args:
req (GetGazeGoalRequest) : Unused
Returns:
GetGazeGoalResponse
'''
goal = self._current_gaze_action
# rospy.loginfo("Gaze Goal: {}".format(goal))
return GetGazeGoalResponse(int(goal))
def _get_ee_pos(self):
'''Get 3DOF pose of end effector
Returns:
Point: location of wrist of end effector
(could change to be finger tips)
'''
from_frame = '/base_link'
to_frame = '/wrist_roll_link'
try:
t = self._tf_listener.getLatestCommonTime(from_frame, to_frame)
(position, rotation) = self._tf_listener.lookupTransform(
from_frame,
to_frame, t)
except:
rospy.logerr('Could not get the end-effector pose.')
return Point(position[0], position[1], position[2])
## Callback function for receiving gaze commands
def _execute_gaze_action(self, goal):
'''Get 3DOF pose of end effector
Args:
goal (GazeGoal): what type of action to perform next
'''
# rospy.loginfo("Got a head goal: {}".format(goal.action))
command = goal.action
if self._no_interrupt.count(self._current_gaze_action) == 0:
if (self._current_gaze_action != command or
command == GazeGoal.LOOK_AT_POINT):
self._is_action_complete = False
if command == GazeGoal.LOOK_FORWARD:
self._target_focus_point = self._default_focus_point
elif command == GazeGoal.LOOK_DOWN:
self._target_focus_point = self._down_focus_point
elif command == GazeGoal.NOD:
self._n_nods = goal.repeat
self._start_nod()
elif command == GazeGoal.SHAKE:
self._n_shakes = goal.repeat
self._start_shake()
elif command == GazeGoal.GLANCE_EE:
self._start_glance()
elif command == GazeGoal.LOOK_AT_POINT:
self._target_focus_point = goal.point
# rospy.loginfo('\tSetting gaze action to: ' +
# self.gaze_goal_strs[command])
self._current_gaze_action = command
while not self._is_action_complete:
time.sleep(0.1)
self._current_gaze_action = GazeGoal.NONE
# Perturb the head goal so it gets updated in the update loop.
self._head_goal.target.point.x += 1
self._gaze_action_server.set_succeeded()
else:
self._gaze_action_server.set_aborted()
else:
self._gaze_action_server.set_aborted()
def _start_nod(self):
'''Start nod action'''
self._prev_target_focus_point = self._target_focus_point
self._prev_gaze_action = str(self._current_gaze_action)
self._nod_counter = 0
self._target_focus_point = self._nod_positions[0]
def _start_glance(self):
'''Start glance action'''
self._prev_target_focus_point = self._target_focus_point
self._prev_gaze_action = str(self._current_gaze_action)
self._glance_counter = 0
self._target_focus_point = self._get_ee_pos()
def _start_shake(self):
'''Start shake action'''
self._prev_target_focus_point = self._target_focus_point
self._prev_gaze_action = str(self._current_gaze_action)
self._shake_counter = 0
self._target_focus_point = self._shake_positions[0]
def _get_next_nod_point(self, current, target):
'''Get next point to look at while nodding
Args:
current (Point): current head goal
target (Point): new head goal
Returns:
Point
'''
if (SocialGaze._is_the_same(SocialGaze._point2array(current),
SocialGaze._point2array(target))):
self._nod_counter += 1
if self._nod_counter == self._n_nods:
self._current_gaze_action = self._prev_gaze_action
return self._prev_target_focus_point
else:
return self._nod_positions[self._nod_counter % 2]
else:
return target
def _get_next_glance_point(self, current, target):
'''Get next point to look at while glancing
Args:
current (Point): current head goal
target (Point): new head goal
Returns:
Point
'''
if (SocialGaze._is_the_same(SocialGaze._point2array(current),
SocialGaze._point2array(target))):
self._glance_counter = 1
self._current_gaze_action = self._prev_gaze_action
return self._prev_target_focus_point
else:
return target
def _get_next_shake_point(self, current, target):
'''Get next point to look at while shaking
Args:
current (Point): current head goal
target (Point): new head goal
Returns:
Point
'''
if (SocialGaze._is_the_same(SocialGaze._point2array(current),
SocialGaze._point2array(target))):
self._shake_counter += 1
if self._shake_counter == self._n_shakes:
self._current_gaze_action = self._prev_gaze_action
return self._prev_target_focus_point
else:
return self._shake_positions[self._shake_counter % 2]
else:
return target
class NavToNode(object):
def __init__(self):
# action client
self._ac_gp = SimpleActionClient("global_planner_node_action",
GlobalPlannerAction)
self._ac_gp.wait_for_server()
self._ac_lp = SimpleActionClient("local_planner_node_action",
LocalPlannerAction)
self._ac_lp.wait_for_server()
# action servergoal
self._as = SimpleActionServer("nav_to_node_action",
NavToAction,
self.ExecuteCB,
auto_start=False)
# define goals to send
self.global_planner_goal_ = GlobalPlannerGoal()
self.local_planner_goal_ = LocalPlannerGoal()
self.new_goal_ = False
self.new_path_ = False
self.global_planner_error_code = -1
self.local_planner_error_code = -1
# new thread running the execution loop
self.thread_ = Thread(target=self.Loop)
self.thread_.start()
self._as.start()
def ExecuteCB(self, goal):
rospy.loginfo('NAVTO: Goal received')
self.global_planner_goal_.goal = goal.navgoal
self.global_planner_error_code = -1
self.local_planner_error_code = -1
self.new_goal_ = True
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
rospy.loginfo('NAVTO: preempt requested')
self._as.set_preempted()
self.new_goal_ = False
self.new_path_ = False
self._ac_gp.cancel_all_goals()
self._ac_lp.cancel_all_goals()
return
if self.global_planner_error_code == 0:
self.new_goal_ = False
self.new_path_ = False
self._ac_lp.wait_for_result()
self._as.set_succeeded()
# rospy.loginfo('NAVTO: succeeded')
break
if self.global_planner_error_code >= 1:
self.new_goal_ = False
self.new_path_ = False
self._ac_lp.wait_for_result()
self._as.set_aborted()
# rospy.loginfo('NAVTO: failed')
break
def Loop(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
self._ac_gp.cancel_all_goals()
self._ac_lp.cancel_all_goals()
if self.new_goal_:
self._ac_gp.send_goal(self.global_planner_goal_,
self.GlobalPlannerDoneCB, None,
self.GlobalPlannerFeedbackCallback)
self.new_goal_ = False
if self.new_path_:
self._ac_lp.send_goal(self.local_planner_goal_,
self.LocalPlannerDoneCallback, None,
self.LocalPlanerFeedbackCallback)
self.new_path_ = False
try:
rate.sleep()
except:
rospy.loginfo('NAVTO: exiting')
def GlobalPlannerDoneCB(self, state, result):
rospy.loginfo("NAVTO: GP, %d, %d" % (state, result.error_code))
self.global_planner_error_code = result.error_code
def GlobalPlannerFeedbackCallback(self, data):
self.local_planner_goal_.route = data.path
self.new_path_ = True
def LocalPlannerDoneCallback(self, state, result):
rospy.loginfo("NAVTO: LP, %d, %d" % (state, result.error_code))
self.local_planner_error_code = result.error_code
def LocalPlanerFeedbackCallback(self, data):
pass
class 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 MotionService(object):
def __init__(self):
rospy.init_node('motion_service')
# Load config
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
# Create publisher for first target
if len(config_loader.targets) > 0:
self._motion_publisher = MotionPublisher(
config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix)
else:
rospy.logerr('Robot config needs to contain at least one valid target.')
self._motion_data = MotionData(config_loader.robot_config)
# Subscriber to start motions via topic
self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb)
# Create action server
self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False)
self._action_server.register_goal_callback(self._execute_motion_goal)
self._action_server.register_preempt_callback(self._preempt_motion_goal)
self._preempted = False
def _execute_motion_goal(self):
goal = self._action_server.accept_new_goal()
# Check if motion exists
if goal.motion_name not in self._motion_data:
result = ExecuteMotionResult()
result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN]
result.error_string = "The requested motion is unknown."
self._action_server.set_aborted(result)
return
# check if a valid time_factor is set, otherwise default to 1.0
if goal.time_factor <= 0.0:
goal.time_factor = 1.0
self._preempted = False
self._motion_publisher.publish_motion(self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished)
print '[MotionService] New action goal received.'
def _preempt_motion_goal(self):
self._motion_publisher.stop_motion()
print '[MotionService] Preempting goal.'
self._preempted = True
def _trajectory_finished(self, error_codes, error_groups):
result = ExecuteMotionResult()
result.error_code = error_codes
error_string = ""
for error_code, error_group in zip(error_codes, error_groups):
error_string += error_group + ': ' + str(error_code) + '\n'
result.error_string = error_string
if self._preempted:
self._action_server.set_preempted(result)
else:
self._action_server.set_succeeded(result)
print '[MotionService] Trajectory finished with error code: \n', error_string
def _execute_motion(self, request):
response = ExecuteMotionResponse()
# check if a motion with this name exists
if request.motion_name not in self._motion_data:
print '[MotionService] Unknown motion name: "%s"' % request.motion_name
response.ok = False
response.finish_time.data = rospy.Time.now()
return response
# check if a valid time_factor is set, otherwise default to 1.0
if request.time_factor <= 0.0:
request.time_factor = 1.0
# find the duration of the requested motion
motion_duration = 0.0
for poses in self._motion_data[request.motion_name].values():
if len(poses) > 0:
endtime = poses[-1]['starttime'] + poses[-1]['duration']
motion_duration = max(motion_duration, endtime)
motion_duration *= request.time_factor
# execute motion
print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (request.motion_name, request.time_factor, motion_duration)
self._motion_publisher.publish_motion(self._motion_data[request.motion_name], request.time_factor)
# reply with ok and the finish_time of this motion
response.ok = True
response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(motion_duration)
return response
def start_server(self):
rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion, self._execute_motion)
self._action_server.start()
print '[MotionService] Waiting for calls...'
rospy.spin()
def motion_command_request_cb(self, command):
print "[MotionService] Initiate motion command via topic ..."
request = ExecuteMotion()
request.motion_name = command.motion_name
request.time_factor = command.time_factor
self._execute_motion(request)
print "[MotionService] Motion command complete"
class SmartArmGripperActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 2 # Number of joints to manage
self.ERROR_THRESHOLD = 0.01 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(5.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME, anonymous=True)
# Initialize publisher & subscriber for left finger
self.left_finger_frame = 'arm_left_finger_link'
self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64)
rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger)
rospy.wait_for_message('finger_left_controller/state', JointControllerState)
# Initialize publisher & subscriber for right finger
self.right_finger_frame = 'arm_right_finger_link'
self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64)
rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger)
rospy.wait_for_message('finger_right_controller/state', JointControllerState)
# Initialize action server
self.result = SmartArmGripperResult()
self.feedback = SmartArmGripperFeedback()
self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback)
# Reset gripper position
rospy.sleep(1)
self.reset_gripper_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def reset_gripper_position(self):
self.left_finger_pub.publish(0.0)
self.right_finger_pub.publish(0.0)
rospy.sleep(5)
def read_left_finger(self, data):
self.left_finger = data
self.has_latest_left_finger = True
def read_right_finger(self, data):
self.right_finger = data
self.has_latest_right_finger = True
def wait_for_latest_controller_states(self, timeout):
self.has_latest_left_finger = False
self.has_latest_right_finger = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_left_finger == False or self.has_latest_right_finger == False) and \
(rospy.Time.now() - start < timeout):
r.sleep()
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
rospy.loginfo("%s: Executing move gripper", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
rospy.loginfo("%s: Aborted: Invalid Goal", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal to controllers
self.left_finger_pub.publish(target_joints[0])
self.right_finger_pub.publish(target_joints[1])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.left_finger.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.right_finger.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current gripper position as feedback
self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
self.server.set_succeeded(self.result)
class 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 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 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 PickNPlaceServer(object):
def __init__(self):
rospy.loginfo('initializing PickNPlaceServer')
# loading config variables
self.dynamic_reconfigure_srv = Server(GraspConfig,
self.dynamic_reconfigure_cb)
# grasp library
rospy.loginfo('connecting to pickup action server')
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo('successfully connected to pickup action server!')
rospy.loginfo('connecting to place action server')
self.place_ac = SimpleActionClient('/place', PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo('successfully connected to place action server!')
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('successfully connected to get_planning_scene service!')
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('successfully connected to clear octomap service!')
self.links_to_allow_contact = rospy.get_param(
'/links_to_allow_contact')
if not self.links_to_allow_contact:
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('/pickup_pose',
PickNPlacePoseAction,
execute_cb=self.pick_cb,
auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer('/place_pose',
PickNPlacePoseAction,
execute_cb=self.place_cb,
auto_start=False)
self.place_as.start()
self.grasp_marker_pub = rospy.Publisher('/grasp_marker',
Marker,
queue_size=10)
self.move_group = MoveGroupCommander('arm_torso')
self.rate = rospy.Rate(10)
self.velocity = Twist()
self.velocity_publisher = rospy.Publisher(
'/mobile_base_controller/cmd_vel', Twist, queue_size=10)
def dynamic_reconfigure_cb(self, config, level):
# load config values for grasping
self._time_pre_grasp_posture = config["time_pre_grasp_posture"]
self._time_grasp_posture = config["time_grasp_posture"]
self._time_grasp_posture_final = config["time_grasp_posture_final"]
self._max_contact_force = config["max_contact_force"]
print(config)
return config
def pick_cb(self, goal):
# check result from grasp_object and set it
error_code = self.grasp_object(goal)
pick_result = PickNPlacePoseResult()
pick_result.error_code = error_code
if error_code != 1:
self.pick_as.set_aborted(pick_result)
else:
self.pick_as.set_succeeded(pick_result)
def place_cb(self, goal):
# check result from place_object and set it
error_code = self.place_object(goal.object_pose)
place_result = PickNPlacePoseResult()
place_result.error_code = error_code
if error_code != 1:
self.place_as.set_aborted(place_result)
else:
self.place_as.set_succeeded(place_result)
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo('waiting for object ' + object_name +
' to appear in planning scene...')
get_planning_scene_request = GetPlanningSceneRequest()
get_planning_scene_request.components.components = get_planning_scene_request.components.WORLD_OBJECT_NAMES
# loop until our object appears on scene
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
get_planning_scene_response = self.scene_srv.call(
get_planning_scene_request)
for collision_object in get_planning_scene_response.scene.world.collision_objects:
if collision_object.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo(object_name + ' is in the scene!')
def open_and_close_gripper(self, grasp_width):
# open or close grippers
pre_grasp_posture = JointTrajectory()
pre_grasp_posture.header.frame_id = 'arm_tool_link'
pre_grasp_posture.joint_names = [
'gripper_left_finger_joint', 'gripper_right_finger_joint'
]
jt_point1 = JointTrajectoryPoint()
jt_point1.positions = [0.05, 0.05]
jt_point1.time_from_start = rospy.Duration(
self._time_pre_grasp_posture)
pre_grasp_posture.points.append(jt_point1)
grasp_posture = deepcopy(pre_grasp_posture)
grasp_posture.points[0].time_from_start = rospy.Duration(
self._time_pre_grasp_posture + self._time_grasp_posture)
jt_point2 = JointTrajectoryPoint()
jt_point2.positions = [grasp_width, grasp_width]
jt_point2.time_from_start = rospy.Duration(
self._time_pre_grasp_posture + self._time_grasp_posture +
self._time_grasp_posture_final)
grasp_posture.points.append(jt_point2)
return {
'pre_grasp_posture': pre_grasp_posture,
'grasp_posture': grasp_posture
}
def create_gripper_translation(self,
direction_vector,
min_distance,
desired_distance,
frame_id='base_footprint'):
# used to generate pre-grasp approach and post-grasp retreat for grasps
g_trans = GripperTranslation()
g_trans.direction.header.frame_id = frame_id
g_trans.direction.vector = direction_vector
g_trans.min_distance = min_distance
g_trans.desired_distance = desired_distance
return g_trans
def create_grasp(self, pose, approach, retreat, grasp_width, grasp_id=''):
g = Grasp()
g.id = grasp_id
g.grasp_pose.header.frame_id = 'base_footprint'
g.grasp_pose.pose = pose
# set direction of approach and retreat pre and post grasping, respectively
g.pre_grasp_approach = self.create_gripper_translation(
approach, 0.095, 0.9)
g.post_grasp_retreat = self.create_gripper_translation(
retreat, 0.1, 0.2)
print('grasp width : ', grasp_width)
gripper_postures = self.open_and_close_gripper(grasp_width)
g.pre_grasp_posture = gripper_postures[
'pre_grasp_posture'] # open gripper before grasp
g.grasp_posture = gripper_postures[
'grasp_posture'] # close gripper during grasp
g.max_contact_force = self._max_contact_force # don't knock the object down whilst grasping
return g
def generate_grasps(self, pnp_goal):
# decide between front or top grasp
eef_gripper_dist = 0.11
gripper_height = 0.11 # objects must be equal or above this for front-grasp
max_graspable = 0.10 # max obj dimension that can fit between the grippers
grasp_depth = 0.06 # max depth between gripper tip to gripper palm
grasps = []
if (pnp_goal.object_width < max_graspable
and pnp_goal.object_height >= gripper_height):
front_grasp_center_pose = Pose()
front_grasp_center_pose.position = copy.deepcopy(
pnp_goal.object_pose.pose.position)
front_grasp_center_pose.orientation = Quaternion(
*quaternion_from_euler(PI / 2, 0.0, 0.0)) # horizontal gripper
front_grasp_center_pose.position.x = front_grasp_center_pose.position.x - (
eef_gripper_dist + (pnp_goal.object_depth / 2))
front_center_approach = Vector3(
1.0, 0.0, 0.0) # approach 'in' for front-grasp
front_center_retreat = Vector3(0.0, 0.0,
1.0) # retreat 'upwards' post-grasp
front_center_grasp_width = pnp_goal.object_width / 2.58
front_center_grasp_width = floor(
front_center_grasp_width * 100) / 100
front_center_grasp = self.create_grasp(
front_grasp_center_pose,
front_center_approach,
front_center_retreat,
front_center_grasp_width,
grasp_id='front_center_grasp')
grasps.append(front_center_grasp)
if (pnp_goal.object_width < max_graspable and pnp_goal.object_height >
(gripper_height / 2.0)):
front_grasp_top_pose = Pose()
front_grasp_top_pose.position = copy.deepcopy(
pnp_goal.object_pose.pose.position)
front_grasp_top_pose.orientation = Quaternion(
*quaternion_from_euler(PI / 2, 0.0, 0.0)) # horizontal gripper
front_grasp_top_pose.position.x = front_grasp_top_pose.position.x - (
eef_gripper_dist + (pnp_goal.object_depth / 2))
front_grasp_top_pose.position.z = front_grasp_top_pose.position.z + (
pnp_goal.object_height / 2) - 0.05
front_top_approach = Vector3(1.0, 0.0,
0.0) # approach 'in' for front-grasp
front_top_retreat = Vector3(0.0, 0.0,
1.0) # retreat 'upwards' post-grasp
front_top_grasp_width = pnp_goal.object_width / 2.58
front_top_grasp_width = floor(front_top_grasp_width * 100) / 100
front_top_grasp = self.create_grasp(front_grasp_top_pose,
front_top_approach,
front_top_retreat,
front_top_grasp_width,
grasp_id='front_top_grasp')
grasps.append(front_top_grasp)
if (pnp_goal.object_width < max_graspable):
top_grasp_width_pose = Pose()
top_grasp_width_pose.position = copy.deepcopy(
pnp_goal.object_pose.pose.position)
top_grasp_width_pose.orientation = Quaternion(
*quaternion_from_euler(PI / 2, PI / 2, 0.0))
top_grasp_width_pose.position.z = top_grasp_width_pose.position.z + (
grasp_depth + eef_gripper_dist + (pnp_goal.object_height / 2))
top_grasp_width_approach = Vector3(
0.0, 0.0, -1.0) # approach 'down' for top-grasp
top_grasp_width_retreat = Vector3(
0.0, 0.0, 1.0) # retreat 'upwards' post-grasp
top_grasp_width_width = pnp_goal.object_width / 2.58
top_grasp_width_width = floor(top_grasp_width_width * 100) / 100
top_grasp_width = self.create_grasp(top_grasp_width_pose,
top_grasp_width_approach,
top_grasp_width_retreat,
top_grasp_width_width,
grasp_id='top_grasp_width')
grasps.append(top_grasp_width)
if (pnp_goal.object_depth < max_graspable):
top_grasp_depth_pose = Pose()
top_grasp_depth_pose.position = copy.deepcopy(
pnp_goal.object_pose.pose.position)
top_grasp_depth_pose.orientation = Quaternion(
*quaternion_from_euler(0.0, PI / 2, 0.0))
top_grasp_depth_pose.position.z = top_grasp_depth_pose.position.z + (
grasp_depth + eef_gripper_dist + (pnp_goal.object_height / 2))
top_grasp_depth_approach = Vector3(
0.0, 0.0, -1.0) # approach 'down' for top-grasp
top_grasp_depth_retreat = Vector3(
0.0, 0.0, 1.0) # retreat 'upwards' post-grasp
top_grasp_depth_width = pnp_goal.object_depth / 2.58
top_grasp_depth_width = floor(top_grasp_depth_width * 100) / 100
top_grasp_depth = self.create_grasp(top_grasp_depth_pose,
top_grasp_depth_approach,
top_grasp_depth_retreat,
top_grasp_depth_width,
grasp_id='top_grasp_depth')
grasps.append(top_grasp_depth)
return grasps
def clear_world_objects(self):
rospy.loginfo("removing any previous 'part' object")
# self.scene.remove_attached_object('arm_tool_link')
self.scene.remove_world_object('part')
# def set_orientation_constraint(self, orientation, frame_id='base_footprint', link_name='arm_tool_link'):
# gripper_constraint = Constraints()
# gripper_constraint.name = 'gripper constraint'
#
# gripper_orientation_constraint = OrientationConstraint()
# gripper_orientation_constraint.header.frame_id = frame_id
# gripper_orientation_constraint.link_name = link_name
# gripper_orientation_constraint.orientation = orientation
# gripper_orientation_constraint.absolute_x_axis_tolerance = 0.01 # allow max rotation of 1 degree
# gripper_orientation_constraint.absolute_y_axis_tolerance = 0.01
# gripper_orientation_constraint.absolute_z_axis_tolerance = 0.01
# gripper_orientation_constraint.weight = 1.0
#
# gripper_constraint.orientation_constraints = [gripper_orientation_constraint]
#
# self.move_group.set_path_constraints(gripper_constraint)
def execute_post_grasp_goal(self, grasp):
# create arm goal with same 'arm_tool' orientation as the attempted grasp
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'base_footprint'
goal_pose.pose.position.x = 0.35
goal_pose.pose.position.y = -0.30
goal_pose.pose.position.z = 0.76
goal_pose.pose.orientation = grasp.grasp_pose.pose.orientation
# set reference frame and send goal
self.move_group.set_pose_reference_frame('base_footprint')
self.move_group.set_pose_target(goal_pose)
self.move_group.go()
def move(self, linear=(0, 0, 0), angular=(0, 0, 0)):
self.velocity.linear.x = linear[
0] # Forward or Backward with in m/sec.
self.velocity.linear.y = linear[1]
self.velocity.linear.z = linear[2]
self.velocity.angular.x = angular[0]
self.velocity.angular.y = angular[1]
self.velocity.angular.z = angular[
2] # Anti-clockwise/clockwise in radians per sec
self.velocity_publisher.publish(self.velocity)
def grasp_object(self, pnp_goal):
self.clear_world_objects()
rospy.loginfo("adding new 'part' object")
self.scene.add_box('part', pnp_goal.object_pose,
(pnp_goal.object_depth, pnp_goal.object_width,
pnp_goal.object_height))
# wait for objects part to appear
self.wait_for_planning_scene_object('part')
# compute grasp
grasps = self.generate_grasps(pnp_goal)
rospy.loginfo('computed grasps :: ')
rospy.loginfo(grasps)
error_code = -1
for grasp in grasps:
# publish grasp marker
print('publishing grasp marker! ' + grasp.id)
if grasp.id.split('_')[0] != 'front':
grasp_marker = Util.visualize_marker(grasp.grasp_pose.pose,
scale=0.02,
type='pose',
color='blue')
else:
grasp_marker = Util.visualize_marker(grasp.grasp_pose.pose,
scale=0.02,
type='pose')
self.grasp_marker_pub.publish(grasp_marker)
goal = create_pickup_goal('arm_torso', 'part',
pnp_goal.object_pose, [grasp],
self.links_to_allow_contact)
rospy.loginfo('sending pick goal')
self.pickup_ac.send_goal(goal)
rospy.loginfo('waiting for result')
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
error_code = result.error_code.val
rospy.logdebug('using torso result: ' + str(result))
rospy.loginfo('pick result: ' + str(moveit_error_dict[error_code]))
if error_code == 1:
# turn around for about 180 degs to be clear of furniture
for i in range(85):
self.move((0, 0, 0), (0, 0, 0.4))
self.rate.sleep()
# move arm close to body after grasp is successful
self.execute_post_grasp_goal(grasp)
break
return error_code
class PutDownAtActionServer:
def __init__(self):
self.start_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/start_audio_recording', StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy(
'/audio_dump/stop_audio_recording', StopAudioRecording)
self.grasp_planning_srv = rospy.ServiceProxy('/GraspPlanning',
GraspPlanning)
self.get_solver_info_srv = rospy.ServiceProxy(
'/wubble2_left_arm_kinematics/get_ik_solver_info',
GetKinematicSolverInfo)
self.get_ik_srv = rospy.ServiceProxy(
'/wubble2_left_arm_kinematics/get_ik', GetPositionIK)
self.set_planning_scene_diff_client = rospy.ServiceProxy(
'/environment_server/set_planning_scene_diff',
SetPlanningSceneDiff)
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status',
GraspStatus)
self.find_cluster_bbox = rospy.ServiceProxy(
'/find_cluster_bounding_box', FindClusterBoundingBox)
self.posture_controller = SimpleActionClient(
'/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.move_arm_client = SimpleActionClient('/move_left_arm',
MoveArmAction)
self.attached_object_pub = rospy.Publisher(
'/attached_collision_object', AttachedCollisionObject)
self.action_server = SimpleActionServer(ACTION_NAME,
PutDownAtAction,
execute_cb=self.put_down_at,
auto_start=False)
def initialize(self):
rospy.loginfo(
'%s: waiting for audio_dump/start_audio_recording service' %
ACTION_NAME)
rospy.wait_for_service('audio_dump/start_audio_recording')
rospy.loginfo(
'%s: connected to audio_dump/start_audio_recording service' %
ACTION_NAME)
rospy.loginfo(
'%s: waiting for audio_dump/stop_audio_recording service' %
ACTION_NAME)
rospy.wait_for_service('audio_dump/stop_audio_recording')
rospy.loginfo(
'%s: connected to audio_dump/stop_audio_recording service' %
ACTION_NAME)
rospy.loginfo('%s: waiting for GraspPlanning service' % ACTION_NAME)
rospy.wait_for_service('/GraspPlanning')
rospy.loginfo('%s: connected to GraspPlanning service' % ACTION_NAME)
rospy.loginfo(
'%s: waiting for wubble2_left_arm_kinematics/get_ik_solver_info service'
% ACTION_NAME)
rospy.wait_for_service(
'/wubble2_left_arm_kinematics/get_ik_solver_info')
rospy.loginfo(
'%s: connected to wubble2_left_arm_kinematics/get_ik_solver_info service'
% ACTION_NAME)
rospy.loginfo(
'%s: waiting for wubble2_left_arm_kinematics/get_ik service' %
ACTION_NAME)
rospy.wait_for_service('/wubble2_left_arm_kinematics/get_ik')
rospy.loginfo(
'%s: connected to wubble2_left_arm_kinematics/get_ik service' %
ACTION_NAME)
rospy.loginfo(
'%s: waiting for environment_server/set_planning_scene_diff service'
% ACTION_NAME)
rospy.wait_for_service('/environment_server/set_planning_scene_diff')
rospy.loginfo('%s: connected to set_planning_scene_diff service' %
ACTION_NAME)
rospy.loginfo('%s: waiting for wubble_gripper_grasp_action' %
ACTION_NAME)
self.posture_controller.wait_for_server()
rospy.loginfo('%s: connected to wubble_gripper_grasp_action' %
ACTION_NAME)
rospy.loginfo('%s: waiting for move_left_arm action server' %
ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server' %
ACTION_NAME)
rospy.loginfo('%s: waiting for wubble_grasp_status service' %
ACTION_NAME)
rospy.wait_for_service('/wubble_grasp_status')
rospy.loginfo('%s: connected to wubble_grasp_status service' %
ACTION_NAME)
rospy.loginfo('%s: waiting for find_cluster_bounding_box service' %
ACTION_NAME)
rospy.wait_for_service('/find_cluster_bounding_box')
rospy.loginfo('%s: connected to find_cluster_bounding_box service' %
ACTION_NAME)
self.action_server.start()
def find_ik_for_grasping_pose(self, pose_stamped):
solver_info = self.get_solver_info_srv()
arm_joints = solver_info.kinematic_solver_info.joint_names
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = GRIPPER_LINK_FRAME
req.ik_request.pose_stamped = pose_stamped
try:
current_state = rospy.wait_for_message('/joint_states', JointState,
2.0)
req.ik_request.ik_seed_state.joint_state.name = arm_joints
req.ik_request.ik_seed_state.joint_state.position = [
current_state.position[current_state.name.index(j)]
for j in arm_joints
]
if not self.set_planning_scene_diff_client():
rospy.logerr(
'%s: Find IK for Grasp: failed to set planning scene diff'
% ACTION_NAME)
return None
ik_result = self.get_ik_srv(req)
if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS:
return ik_result.solution
else:
rospy.logerr(
'%s: failed to find an IK for requested grasping pose, aborting'
% ACTION_NAME)
return None
except:
rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME)
return None
def find_grasp_pose(self,
target,
collision_object_name='',
collision_support_surface_name=''):
"""
target = GraspableObject
collision_object_name = name of target in collision map
collision_support_surface_name = name of surface target is touching
"""
req = GraspPlanningRequest()
req.arm_name = ARM_GROUP_NAME
req.target = target
req.collision_object_name = collision_object_name
req.collision_support_surface_name = collision_support_surface_name
rospy.loginfo(
'%s: trying to find a good grasp for graspable object %s' %
(ACTION_NAME, collision_object_name))
grasping_result = self.grasp_planning_srv(req)
if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS:
rospy.logerr('%s: unable to find a feasable grasp, aborting' %
ACTION_NAME)
return None
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = grasping_result.grasps[
0].grasp_posture.header.frame_id
pose_stamped.pose = grasping_result.grasps[0].grasp_pose
rospy.loginfo('%s: found good grasp, looking for corresponding IK' %
ACTION_NAME)
return self.find_ik_for_grasping_pose(pose_stamped)
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
def close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
rospy.sleep(1)
grasp_status = self.get_grasp_status_srv()
return grasp_status.is_hand_occupied
def find_cluster_bounding_box_center(self, cluster):
fcbb = FindClusterBoundingBoxRequest()
fcbb.cluster = cluster
res = self.find_cluster_bbox(fcbb)
if res.error_code == FindClusterBoundingBoxResponse.TF_ERROR:
rospy.logerr(
'Unable to find bounding box for requested point cluster')
return None
return res.pose.pose.position
def put_down_at(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
bbox_center = self.find_cluster_bounding_box_center(
goal.graspable_object.cluster)
if not bbox_center: self.action_server.set_aborted()
goal.target_location.z = bbox_center.z
x_dist = goal.target_location.x - bbox_center.x
y_dist = goal.target_location.y - bbox_center.y
target = goal.graspable_object
target.cluster.points = [
Point32(p.x + x_dist, p.y + y_dist, p.z)
for p in target.cluster.points
]
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
ik_solution = self.find_grasp_pose(target, collision_object_name,
collision_support_surface_name)
if ik_solution:
# disable collisions between gripper and target object
ordered_collision_operations = OrderedCollisionOperations()
collision_operation = CollisionOperation()
collision_operation.object1 = collision_object_name
collision_operation.object2 = GRIPPER_GROUP_NAME
collision_operation.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.01
# disable collisions between arm and table
collision_operation3 = CollisionOperation()
collision_operation3.object1 = collision_support_surface_name
collision_operation3.object2 = ARM_GROUP_NAME
collision_operation3.operation = CollisionOperation.DISABLE
collision_operation3.penetration_distance = 0.01
ordered_collision_operations.collision_operations = [
collision_operation, collision_operation2, collision_operation3
]
# set gripper padding to 0
gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
gripper_paddings.extend([LinkPadding(l, 0.0) for l in ARM_LINKS])
# move into pre-grasp pose
if not move_arm_joint_goal(
self.move_arm_client,
ik_solution.joint_state.name,
ik_solution.joint_state.position,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations):
rospy.logerr('%s: attempted grasp failed' % ACTION_NAME)
self.action_server.set_aborted()
return
# record put down sound with 0.5 second padding before and after
self.start_audio_recording_srv(InfomaxAction.GRASP,
goal.category_id)
rospy.sleep(0.5)
self.open_gripper()
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
self.action_server.set_succeeded(
PutDownAtResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr('%s: attempted put down failed' % ACTION_NAME)
self.action_server.set_aborted()
class FakeGrasping:
ALWAYS = 0
NEVER = 1
RANDOM = 2
def __init__(self):
self.server = SimpleActionServer('/robot_move',
RobotMoveAction,
execute_cb=self.robot_move_cb)
self.objects = self.ALWAYS
self.grasp = self.ALWAYS
self.place = self.ALWAYS
self.object_randomness = 0.8 # 80% of time object is known
self.grasp_randomness = 0.4
self.place_randomness = 0.4
self.holding = None
self.pick_length = 2 # how long (sec) takes to pick an object
self.place_length = 2 # how long (sec) takes to place an object
self.robot_state_pub = rospy.Publisher("/robot_status",
RobotStatus,
queue_size=1,
latch=True)
self.publish_robot_status(RobotStatus.STOPPED)
random.seed()
def publish_robot_status(self, status):
self.robot_state_pub.publish(
RobotStatus(
status=status,
actual_pose=PoseStamped(header=Header(frame_id="marker"))))
def robot_move_cb(self, goal):
self.publish_robot_status(RobotStatus.MOVING)
self.robot_move(goal)
self.publish_robot_status(RobotStatus.STOPPED)
def robot_move(self, goal):
result = RobotMoveResult()
feedback = RobotMoveFeedback()
if not (goal.move_type in (goal.HOME, goal.PROGRAMING, goal.PICK,
goal.PLACE, goal.MOVE)):
result.result = RobotMoveResult.BAD_REQUEST
rospy.logerr("BAD_REQUEST, Unknown operation")
self.server.set_aborted(result, "Unknown operation")
return
if self.objects == self.ALWAYS:
pass
elif self.objects == self.NEVER:
result.result = RobotMoveResult.BAD_REQUEST
rospy.logerr("BAD_REQUEST, Unknown object id")
self.server.set_aborted(result, "Unknown object id")
return
elif self.objects == self.RANDOM:
nmbr = random.random()
if nmbr > self.object_randomness:
result.result = RobotMoveResult.BAD_REQUEST
rospy.logerr("BAD_REQUEST, Unknown object id")
self.server.set_aborted(result, "Unknown object id")
return
grasped = False
if goal.move_type == RobotMoveGoal.PICK:
rospy.sleep(self.pick_length)
if False and self.holding:
result.result = RobotMoveResult.BUSY
rospy.logerr("Failure, already holding object in arm")
self.server.set_aborted(result,
"Already holding object in arm")
return
if self.grasp == self.ALWAYS:
grasped = True
pass
elif self.grasp == self.NEVER:
result.result = RobotMoveResult.MOVE_FAILURE
rospy.logerr("FAILURE, Pick Failed")
self.server.set_aborted(result, "Pick Failed")
return
tries = 5
while tries > 0:
feedback.state = feedback.PICKING
tries -= 1
self.server.publish_feedback(feedback)
if self.grasp == self.RANDOM:
nmbr = random.random()
if nmbr < self.grasp_randomness:
grasped = True
if grasped:
break
if self.server.is_preempt_requested():
self.server.set_preempted(result, "Pick canceled")
rospy.logerr("Preempted")
return
if not grasped:
result.result = RobotMoveResult.FAILURE
self.server.set_aborted(result, "Pick failed")
rospy.logerr("FAILURE, Pick Failed")
return
else:
self.holding = True
placed = False
if goal.move_type == RobotMoveGoal.PLACE:
rospy.sleep(self.place_length)
if not self.holding:
result.result = RobotMoveResult.MOVE_FAILURE
rospy.logerr("Failure, robot not holding object")
self.server.set_aborted(result, "Robot not holding object")
return
if self.place == self.ALWAYS:
placed = True
pass
elif self.place == self.NEVER:
result.result = RobotMoveResult.MOVE_FAILURE
self.server.set_aborted(result, "Place Failed")
rospy.logerr("FAILURE, Place Failed")
return
tries = 5
while tries > 0:
feedback.state = feedback.PLACING
tries -= 1
self.server.publish_feedback(feedback)
if self.place == self.RANDOM:
nmbr = random.random()
if nmbr < self.place_randomness:
placed = True
if placed:
break
if not placed:
result.result = RobotMoveResult.MOVE_FAILURE
self.server.set_aborted(result, "Place failed")
rospy.logerr("FAILURE, Place Failed")
return
else:
self.holding = False
result.result = RobotMoveResult.SUCCES
self.server.set_succeeded(result)
rospy.loginfo("SUCCESS")
print("Finished")
class 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 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 PickAndPlaceServer(object):
def __init__(self):
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
rospy.loginfo("Connecting to place AS")
self.place_ac = SimpleActionClient('/place', PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
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 object size
self.object_height = rospy.get_param('~object_height')
self.object_width = rospy.get_param('~object_width')
self.object_depth = rospy.get_param('~object_depth')
# 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(
'/pickup_pose', PickUpPoseAction,
execute_cb=self.pick_cb, auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer(
'/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")
self.scene.remove_world_object("left")
self.scene.remove_world_object("right")
self.scene.remove_world_object("up")
self.scene.remove_world_object("back")
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 = 1.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
table_width = 1.8
table_depth = 0.8
table_pose.pose.position.z += -(2*self.object_height)/2 -table_height/2 +0.11 #before also -0.1 and was object width instead of object height
table_height -= 0.1 #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")
right_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
right_height = 1.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
right_width = 1.8
right_depth = 0.8
right_pose.pose.position.y += 0.5 #before also -0.1 and was object width instead of object height
right_width = 0.2 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("right", right_pose, (right_depth, right_width, right_height))
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("right")
left_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
left_height = 0.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
left_width = 0.8
left_depth = 0.2
left_pose.pose.position.x += 0.7 #before also -0.1 and was object width instead of object height
self.scene.add_box("back", left_pose, (left_depth, left_width, left_height))
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("back")
up_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
up_height = 0.2 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
up_width = 1.8
up_depth = 0.8
up_pose.pose.position.z += 0.5 #before also -0.1 and was object width instead of object height
self.scene.add_box("up", up_pose, (up_depth, up_width, up_height))
back_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
back_height = 1.8 #object_pose.pose.position.z - self.object_width/2 #before it was 1.8
back_width = 1.8
back_depth = 0.8
back_pose.pose.position.y -= 0.5 #before also -0.1 and was object width instead of object height
back_width = 0.2 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("left", back_pose, (back_depth, back_width, back_height))
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("left")
# compute grasps
possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
self.pickup_ac
goal = createPickupGoal(
"arm", "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]))
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", "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 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 BakerArmServer(script):
DOF = 5
DEFAULT_POSITION = [0.6, 1., -2., 1., 0.]
TRANSPORT_POSITION = [0.9, 1., -2., 1., 0.]
IS_GRIPPER_AVAILABLE = False
def __init__(self, name, status=ArmStatus.NO_TRASHCAN):
self.verbose_ = True
self.confirm_ = False
self.name_ = name
self.status_ = status
self.joint_values_ = [0.0] * self.DOF
self.mutex_ = Lock()
(self.trashcan_pose_, self.trolley_pose_) = (None, None)
self.displayParameters()
self.initClients()
self.initServices()
self.initServers()
self.initSubscribers()
self.initTopics()
def confirm(self):
if not self.confirm_:
return
raw_input("Please press enter to confirm")
def displayParameters(self):
if not self.verbose_:
return
print("========== baker_arm_server Parameters ==========")
print("todo")
@log
def Initialize(self):
self.sss.init('gripper')
self.sss.init('arm')
@log
def initClients(self):
# Warning: wait_for_server and wait_for_service are not the same function.
# * rospy.duration expected for wait_for_server
# * float expected for wait_for_service
self.plan_client_ = SimpleActionClient('/arm_planning_node/PlanTo',
PlanToAction)
self.plan_client_.wait_for_server(timeout=rospy.Duration(5.0))
self.execution_client_ = SimpleActionClient('/traj_exec',
ExecuteTrajectoryAction)
self.execution_client_.wait_for_server(timeout=rospy.Duration(5.0))
self.small_gripper_finger_client_ = rospy.ServiceProxy(
"/gripper/driver/set_object", SetObject)
# rmb-ma. keep commented
# self.small_gripper_finger_client_.wait_for_service(timeout=5.0)
self.add_collision_object_client_ = rospy.ServiceProxy(
"/ipa_planning_scene_creator/add_collision_objects",
AddCollisionObject)
self.add_collision_object_client_.wait_for_service(timeout=5.0)
self.attach_object_client_ = rospy.ServiceProxy(
"ipa_planning_scene_creator/attach_object", AddCollisionObject)
self.attach_object_client_.wait_for_service(timeout=5.0)
self.detach_object_client_ = rospy.ServiceProxy(
"ipa_planning_scene_creator/detach_object", RemoveCollisionObject)
self.detach_object_client_.wait_for_service(timeout=5.0)
@log
def initServers(self):
self.to_joints_position_server_ = SimpleActionServer(
self.name_ + '/set_joints_values', ExecuteTrajectoryAction,
self.moveToJointsPositionCallback, False)
self.to_joints_position_server_.start()
self.to_transport_position_server_ = SimpleActionServer(
self.name_ + '/transport_position', MoveToAction,
self.moveToTransportPositionCallback, False)
self.to_transport_position_server_.start()
self.to_rest_position_server_ = SimpleActionServer(
self.name_ + '/rest_position', MoveToAction,
self.moveToRestPositionCallback, False)
self.to_rest_position_server_.start()
self.catch_trashcan_server_ = SimpleActionServer(
self.name_ + '/catch_trashcan', MoveToAction,
self.catchTrashcanCallback, False)
self.catch_trashcan_server_.start()
self.leave_trashcan_server_ = SimpleActionServer(
self.name_ + '/leave_trashcan', MoveToAction,
self.leaveTrashcanCallback, False)
self.leave_trashcan_server_.start()
self.empty_trashcan_server_ = SimpleActionServer(
self.name_ + '/empty_trashcan', MoveToAction,
self.emptyTrashcanCallback, False)
self.empty_trashcan_server_.start()
@log
def initServices(self):
pass
@log
def initTopics(self):
Thread(target=self.statusTalker).start()
@log
def statusTalker(self):
publisher = rospy.Publisher(self.name_ + '/status',
Int32,
queue_size=10)
rate = rospy.Rate(5)
while not rospy.is_shutdown():
publisher.publish(self.status_.value)
rate.sleep()
@log
def initSubscribers(self):
rospy.Subscriber("/arm/joint_states", JointState,
self.jointStateCallback)
def jointStateCallback(self, msg):
'''
getting current position of the joint
@param msg containts joint position, velocity, effort, names
'''
for i in range(0, len(msg.position)):
self.joint_values_[i] = msg.position[i]
def gripperHandler(self,
finger_value=0.01,
finger_open=False,
finger_close=True):
'''
Gripper handler function use to control gripper open/close command,
move parallel part of gripper with give values (in cm)
@param finger_value is the big parallel part move realtive to zero position
@finger_open open the small finger
@finger_close close the small finger
'''
if finger_open or finger_close:
# set_object request, both the finger move together (at the same time)
request = SetObjectRequest()
response = SetObjectResponse()
request.node = 'gripper_finger1_joint'
request.object = '22A0'
if finger_open:
request.value = '5'
elif finger_close:
request.value = '10'
request.cached = False
response = self.small_gripper_finger_client_.call(request)
if response.success:
rospy.loginfo(response.message)
else:
rospy.logerr(response.message)
rospy.logerr("SetObject service call Failed!!")
# default blocking is true, wait until if finishes it tasks
self.sss.move('gripper', [[finger_value]])
return response.success
@log
def handleCheckAccessibility(self, request):
response = TriggerResponse()
response.success = True
return response
@log
def openGripper(self):
if not self.IS_GRIPPER_AVAILABLE:
return
self.gripperHandler(finger_value=0.01,
finger_open=True,
finger_close=False)
@log
def closeGripper(self):
if not self.IS_GRIPPER_AVAILABLE:
return
self.gripperHandler(finger_value=-0.01,
finger_open=False,
finger_close=True)
@log
def executeTrajectory(self, trajectory, server):
goal = ExecuteTrajectoryGoal()
goal.trajectory = trajectory
self.execution_client_.send_goal(goal)
while self.execution_client_.get_state() < 3:
rospy.sleep(0.1)
if not server.is_preempt_requested() and not rospy.is_shutdown():
continue
self.execution_client_.cancel_goal()
self.execution_client_.wait_for_result()
return True
result = self.execution_client_.get_result()
state = self.execution_client_.get_state()
if result.success and state == 3:
rospy.logwarn("Execution finish with given time")
return True
else:
rospy.logerr("Execution aborted!!")
return False
@log
def isPoseAccessible(self, target_pose):
(_, is_planned) = planTrajectoryInCartSpace(client=self.plan_client_,
object_pose=target_pose,
bdmp_goal=None,
bdmp_action='')
return is_planned
@log
def planAndExecuteTrajectoryInJointSpaces(self, target_joints, server):
(trajectory,
is_planned) = planTrajectoryInJointSpace(client=self.plan_client_,
object_pose=target_joints,
bdmp_goal=None,
bdmp_action='')
if not is_planned:
rospy.logerr('Trajectory in joint spaces execution failed')
raise Exception('Trajectory in joint spaces execution failed')
self.confirm()
is_execution_successful = self.executeTrajectory(trajectory=trajectory,
server=server)
if not is_execution_successful:
rospy.logerr('Can not find valid trajectory at joint space')
raise Exception('Trajectory Execution failed')
return not server.is_preempt_requested()
@log
def planAndExecuteTrajectoryInCartesianSpace(self, target_pose, server):
(trajectory,
is_planned) = planTrajectoryInCartSpace(client=self.plan_client_,
object_pose=target_pose,
bdmp_goal=None,
bdmp_action='')
if not is_planned:
rospy.logerr('Cannot find valid trajectory at cartesianSpace')
raise Exception('Cannot find valid trajectory at cartesian space')
self.confirm()
is_execution_successful = self.executeTrajectory(trajectory=trajectory,
server=server)
if not is_execution_successful:
rospy.logerr("Trajectory Execution Failed")
raise Exception('Trajectory Execution Failed')
return not server.is_preempt_requested()
@staticmethod
def poseToLists(pose):
position = [pose.position.x, pose.position.y, pose.position.z]
orientation = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
return (position, orientation)
@log
def catchTrashcanCallback(self, goal):
rospy.sleep(3)
result = MoveToResult()
result.arrived = False
target_pose = goal.target_pos
target_pose.pose.position.z *= 1.1
#The arm canot carry a new trashcan as it already carries one
if self.status_ != ArmStatus.NO_TRASHCAN:
self.catch_trashcan_server_.set_aborted(result)
try:
self.planAndExecuteTrajectoryInCartesianSpace(
target_pose, self.catch_trashcan_server_)
except Exception as e:
print(e)
self.catch_trashcan_server_.set_aborted(result)
return
if self.catch_trashcan_server_.is_preempt_requested():
result.arrived = True
self.catch_trashcan_server_.set_preempted(result)
return
self.closeGripper()
#self.fixTrashCanToRobot()
self.status_ = ArmStatus.FULL_TRASHCAN
# todo rmb-ma setup the grapping move with a real trashcan
# Help:
# try:
# target_pose = make_pose(position=[0.00,-0.00,-0.07], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper')
# self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_)
# except:
# self.catch_trashcan_server_.set_aborted(result)
# return
#
# if self.catch_trashcan_server_.is_preempt_requested():
# result.arrived = True
# self.catch_trashcan_server_.set_preempted(result)
# return
#
# try:
# target_pose = make_pose(position=[0.020,-0.00,0.00], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper')
# self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_)
# except:
# self.catch_trashcan_server_.set_aborted(result)
# return
#
# if self.catch_trashcan_server_.is_preempt_requested():
# result.arrived = True
# self.catch_trashcan_server_.set_preempted(result)
# return
#
# try:
# target_pose = make_pose(position=[0.000,-0.00,0.030], orientation=[0.0,0.0,0.0,1.0], frame_id='gripper')
# self.planAndExecuteTrajectoryInCartesianSpace(target_pose, self.catch_trashcan_server_)
# except:
# self.catch_trashcan_server_.set_aborted(result)
# return
#
result.arrived = True
self.catch_trashcan_server_.set_succeeded(result)
@log
def emptyTrashcanCallback(self, goal):
result = MoveToResult()
#Trashcan emptying while the arm doesnt carry any trashcan
if self.status_ != ArmStatus.FULL_TRASHCAN:
result.arrived = False
self.empty_trashcan_server_.set_aborted(result)
return
try:
self.planAndExecuteTrajectoryInCartesianSpace(
goal.target_pos, self.empty_trashcan_server_)
except:
result.arrived = False
self.empty_trashcan_server_.set_aborted(result)
return
if self.empty_trashcan_server_.is_preempt_requested():
self.empty_trashcan_server_.set_preempted()
return
try:
target_joints = self.joint_values_[0:-1] + [3.]
self.planAndExecuteTrajectoryInJointSpaces(
target_joints, self.empty_trashcan_server_)
except:
result.arrived = False
self.empty_trashcan_server_.set_aborted(result)
return
if self.empty_trashcan_server_.is_preempt_requested():
self.empty_trashcan_server_.set_preempted()
return
rospy.sleep(5)
self.status_ = ArmStatus.FULL_TRASHCAN
target_joints = self.joint_values_[0:-1] + [0]
self.planAndExecuteTrajectoryInJointSpaces(target_joints,
self.empty_trashcan_server_)
result.arrived = True
self.empty_trashcan_server_.set_succeeded(result)
@log
def moveToRestPositionCallback(self, goal):
result = MoveToResult()
# Rest position is unavailable if the arm carries a trashcan
if self.status_ != ArmStatus.NO_TRASHCAN:
result.arrived = False
self.to_rest_position_server_.set_aborted(result)
return
try:
self.planAndExecuteTrajectoryInJointSpaces(
self.DEFAULT_POSITION, self.to_rest_position_server_)
except Exception as e:
rospy.logerr(e)
result.arrived = False
self.to_rest_position_server_.set_aborted(result)
return
if self.to_rest_position_server_.is_preempt_requested():
self.to_rest_position_server_.set_preempted()
return
self.to_rest_position_server_.set_succeeded()
@log
def moveToTransportPositionCallback(self, goal):
result = MoveToResult()
# Transport position is unavailable if the arm doesnt carry a trashcan
if self.status_ == ArmStatus.NO_TRASHCAN:
result.arrived = False
self.to_transport_position_server_.set_aborted(result)
return
try:
result.arrived = self.planAndExecuteTrajectoryInJointSpaces(
self.TRANSPORT_POSITION, self.to_transport_position_server_)
except:
result.arrived = False
self.to_transport_position_server_.set_aborted(result)
return
if self.to_transport_position_server_.is_preempt_requested():
self.to_transport_position_server_.set_preempted()
return
self.to_transport_position_server_.set_succeeded(result)
@log
def leaveTrashcanCallback(self, goal):
result = MoveToResult()
# The arm cannot leave a trashcan if it doesnt carry one
if self.status_ != ArmStatus.EMPTY_TRASHCAN:
result.arrived = False
self.leave_trashcan_server_.set_aborted(result)
return
self.planAndExecuteTrajectoryInCartesianSpace(
goal.target_pos, self.leave_trashcan_server_)
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
self.openGripper()
self.status_ = ArmStatus.NO_TRASHCAN
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
target_pose = make_pose(position=[-0.07, -0., -0.04],
orientation=[0., 0., 0., 1.],
frame_id='gripper')
self.planAndExecuteTrajectoryInCartesianSpace(
target_pose, self.leave_trashcan_server_)
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
target_pose = make_pose(position=[-0.0, -0., 0.05],
orientation=[0., 0., 0., 1.],
frame_id='gripper')
self.planAndExecuteTrajectoryInCartesianSpace(
target_pose, self.leave_trashcan_server_)
if self.leave_trashcan_server_.is_preempt_requested():
self.leave_trashcan_server_.set_preempted()
return
result.arrived = True
self.leave_trashcan_server_.set_succeeded(result)
@log
def moveToJointsPositionCallback(self, goal):
target_joints = goal.trajectory.joint_trajectory.points[0].positions
self.planAndExecuteTrajectoryInJointSpaces(
target_joints, self.to_joints_position_server_)
if self.to_joints_position_server_.is_preempt_requested():
self.to_joints_position_server_.set_preempted()
return
self.to_joints_position_server_.set_succeeded()
class 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 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 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 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 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 Approach(object):
def __init__(self, name, takeoff_height):
self.robot_name = name
self.takeoff_height = takeoff_height
# Mutual exclusion
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
# Create trajectory server
self.trajectory_server = SimpleActionServer(
'approach_server', ExecuteDroneApproachAction, self.goCallback,
False)
self.server_feedback = ExecuteDroneApproachFeedback()
self.server_result = ExecuteDroneApproachResult()
# Get client from hector_quadrotor_actions
self.move_client = SimpleActionClient("/{}/action/pose".format(name),
PoseAction)
self.move_client.wait_for_server()
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("/{}/ground_truth/state".format(name), Odometry,
self.poseCallback)
# Subscribe to topic to receive the planned trajectory
rospy.Subscriber("/{}/move_group/display_planned_path".format(name),
DisplayTrajectory, self.planCallback)
#Auxiliary variables
self.trajectory = [] # Array with the trajectory to be executed
self.trajectory_received = False # Flag to signal trajectory received
self.odom_received = False # Flag to signal odom received
self.robot = RobotCommander(
robot_description="{}/robot_description".format(name),
ns="/{}".format(name))
self.display_trajectory_publisher = rospy.Publisher(
'/{}/move_group/display_planned_path'.format(name),
DisplayTrajectory,
queue_size=20)
# Variables for collision callback
self.validity_srv = rospy.ServiceProxy(
'/{}/check_state_validity'.format(name), GetStateValidity)
self.validity_srv.wait_for_service()
self.collision = False
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
#Start move_group
self.move_group = MoveGroup('earth', name)
self.move_group.set_planner(planner_id='RRTConnectkConfigDefault',
attempts=10,
allowed_time=2) #RRTConnectkConfigDefault
self.move_group.set_workspace([XMIN, YMIN, ZMIN, XMAX, YMAX,
ZMAX]) # Set the workspace size
# Get current robot position to define as start planning point
self.current_pose = self.robot.get_current_state()
#Start planningScenePublisher
self.scene_pub = PlanningScenePublisher(name, self.current_pose)
# Start trajectory server
self.trajectory_server.start()
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.notify()
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
# print(self.odometry)
self.odometry_me.release()
self.odom_received = True
def goCallback(self, pose):
'''
Require a plan to go to the desired target and try to execute it 5 time or return erro
'''
self.target = pose.goal
####################################################################
# First takeoff if the drone is close to ground
self.sonar_me.acquire()
while not (self.current_height and self.odometry):
self.sonar_me.wait()
if self.current_height < self.takeoff_height:
self.odometry_me.acquire()
takeoff_pose = PoseGoal()
takeoff_pose.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
takeoff_pose.target_pose.pose.position.x = self.odometry.position.x
takeoff_pose.target_pose.pose.position.y = self.odometry.position.y
takeoff_pose.target_pose.pose.position.z = self.odometry.position.z + self.takeoff_height - self.current_height #Add the heght error to the height
takeoff_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
takeoff_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
takeoff_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
takeoff_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.odometry_me.release()
self.move_client.send_goal(takeoff_pose)
self.move_client.wait_for_result()
result = self.move_client.get_state()
if result == GoalStatus.ABORTED:
rospy.logerr("Abort approach! Unable to execute takeoff!")
self.trajectory_server.set_aborted()
return
self.sonar_me.release()
####################################################################
rospy.loginfo("Try to start from [{},{},{}]".format(
self.odometry.position.x, self.odometry.position.y,
self.odometry.position.z))
rospy.loginfo("Try to go to [{},{},{}]".format(self.target.position.x,
self.target.position.y,
self.target.position.z))
self.trials = 0
while self.trials < 5:
rospy.logwarn("Attempt {}".format(self.trials + 1))
if (self.trials > 1):
self.target.position.z += 2 * rd() - 1
result = self.go(self.target)
if (result == 'replan') or (result == 'no_plan'):
self.trials += 1
else:
self.trials = 10
self.collision = False
if result == 'ok':
self.trajectory_server.set_succeeded()
elif (result == 'preempted'):
self.trajectory_server.set_preempted()
else:
self.trajectory_server.set_aborted()
def go(self, target_):
'''
Function to plan and execute the trajectory one time
'''
# Insert goal position on an array
target = []
target.append(target_.position.x)
target.append(target_.position.y)
target.append(target_.position.z)
target.append(target_.orientation.x)
target.append(target_.orientation.y)
target.append(target_.orientation.z)
target.append(target_.orientation.w)
#Define target for move_group
# self.move_group.set_joint_value_target(target)
self.move_group.set_target(target)
self.odometry_me.acquire()
self.current_pose.multi_dof_joint_state.transforms[
0].translation.x = self.odometry.position.x
self.current_pose.multi_dof_joint_state.transforms[
0].translation.y = self.odometry.position.y
self.current_pose.multi_dof_joint_state.transforms[
0].translation.z = self.odometry.position.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.x
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.y
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.z
self.current_pose.multi_dof_joint_state.transforms[
0].rotation.x = self.odometry.orientation.w
self.odometry_me.release()
#Set start state
self.move_group.set_start_state(self.current_pose)
# Plan a trajectory till the desired target
plan = self.move_group.plan()
if plan.planned_trajectory.multi_dof_joint_trajectory.points: # Execute only if has points on the trajectory
while (not self.trajectory_received):
rospy.loginfo("Waiting for trajectory!")
rospy.sleep(0.2)
# rospy.loginfo("TRAJECTORY: {}".format(self.trajectory))
#Execute trajectory with action_pose
last_pose = self.trajectory[0]
for pose in self.trajectory:
# Verify preempt call
if self.trajectory_server.is_preempt_requested():
self.move_client.send_goal(last_pose)
self.move_client.wait_for_result()
self.scene_pub.publishScene()
self.trajectory_received = False
self.odom_received = False
return 'preempted'
#Send next pose to move
self.next_pose = pose.target_pose.pose
self.move_client.send_goal(pose,
feedback_cb=self.collisionCallback)
self.move_client.wait_for_result()
result = self.move_client.get_state()
self.scene_pub.publishScene()
# Abort if the drone can not reach the position
if result == GoalStatus.ABORTED:
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'aborted'
elif result == GoalStatus.PREEMPTED:
# last_pose.target_pose.pose.position.z += rd() - 0.5
self.move_client.send_goal(
last_pose) #Go back to the last pose
self.move_client.wait_for_result()
self.trajectory_received = False
self.odom_received = False
return 'replan'
elif result == GoalStatus.SUCCEEDED:
self.trials = 0
last_pose = pose
self.server_feedback.current_pose = self.odometry
self.trajectory_server.publish_feedback(self.server_feedback)
# Reset control variables
self.trajectory_received = False
self.odom_received = False
rospy.loginfo("Trajectory is traversed!")
return 'ok'
else:
rospy.logerr("Trajectory is empty. Planning was unsuccessful.")
return 'no_plan'
def planCallback(self, msg):
'''
Receive planned trajectories and insert it into an array of waypoints
'''
if (not self.odom_received):
return
# Variable to calculate the distance difference between 2 consecutive points
last_pose = PoseGoal()
last_pose.target_pose.pose.position.x = self.odometry.position.x
last_pose.target_pose.pose.position.y = self.odometry.position.y
last_pose.target_pose.pose.position.z = self.odometry.position.z
last_pose.target_pose.pose.orientation.x = self.odometry.orientation.x
last_pose.target_pose.pose.orientation.y = self.odometry.orientation.y
last_pose.target_pose.pose.orientation.z = self.odometry.orientation.z
last_pose.target_pose.pose.orientation.w = self.odometry.orientation.w
self.trajectory = []
last_motion_theta = 0
for t in msg.trajectory:
for point in t.multi_dof_joint_trajectory.points:
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
# Orientate the robot always to the motion direction
delta_x = point.transforms[
0].translation.x - last_pose.target_pose.pose.position.x
delta_y = point.transforms[
0].translation.y - last_pose.target_pose.pose.position.y
motion_theta = atan2(delta_y, delta_x)
last_motion_theta = motion_theta
# Make the robot orientation fit with the motion orientation if the movemente on xy is bigger than RESOLUTION
# if (abs(delta_x) > RESOLUTION) or (abs(delta_y) > RESOLUTION):
q = quaternion_from_euler(0, 0, motion_theta)
waypoint.target_pose.pose.orientation.x = q[0]
waypoint.target_pose.pose.orientation.y = q[1]
waypoint.target_pose.pose.orientation.z = q[2]
waypoint.target_pose.pose.orientation.w = q[3]
# else:
# waypoint.target_pose.pose.orientation.x = point.transforms[0].rotation.x
# waypoint.target_pose.pose.orientation.y = point.transforms[0].rotation.y
# waypoint.target_pose.pose.orientation.z = point.transforms[0].rotation.z
# waypoint.target_pose.pose.orientation.w = point.transforms[0].rotation.w
# Add a rotation inplace if next position has an angle difference bigger than 45
if abs(motion_theta - last_motion_theta) > 0.785:
last_pose.target_pose.pose.orientation = waypoint.target_pose.pose.orientation
self.trajectory.append(last_pose)
last_pose = copy.copy(
waypoint) # Save pose to calc the naxt delta
last_motion_theta = motion_theta
self.trajectory.append(waypoint)
#Insert a last point to ensure that the robot end at the right position
waypoint = PoseGoal()
waypoint.target_pose.header.frame_id = "{}/world".format(
self.robot_name)
waypoint.target_pose.pose.position.x = point.transforms[
0].translation.x
waypoint.target_pose.pose.position.y = point.transforms[
0].translation.y
waypoint.target_pose.pose.position.z = point.transforms[
0].translation.z
waypoint.target_pose.pose.orientation.x = point.transforms[
0].rotation.x
waypoint.target_pose.pose.orientation.y = point.transforms[
0].rotation.y
waypoint.target_pose.pose.orientation.z = point.transforms[
0].rotation.z
waypoint.target_pose.pose.orientation.w = point.transforms[
0].rotation.w
self.trajectory.append(waypoint)
self.trajectory_received = True
def collisionCallback(self, feedback):
'''
This callback runs on every feedback message received
'''
validity_msg = GetStateValidityRequest(
) # Build message to verify collision
validity_msg.group_name = PLANNING_GROUP
if self.next_pose and (not self.collision):
self.odometry_me.acquire()
x = self.odometry.position.x
y = self.odometry.position.y
z = self.odometry.position.z
# Distance between the robot and the next position
dist = sqrt((self.next_pose.position.x - x)**2 +
(self.next_pose.position.y - y)**2 +
(self.next_pose.position.z - z)**2)
# Pose to verify collision
pose = Transform()
pose.rotation.x = self.odometry.orientation.x
pose.rotation.y = self.odometry.orientation.y
pose.rotation.z = self.odometry.orientation.z
pose.rotation.w = self.odometry.orientation.w
self.odometry_me.release()
#Verify possible collisions on diferent points between the robot and the goal point
# rospy.logerr("\n\n\nCOLLISION CALLBACK: ")
# rospy.logerr(dist)
for d in arange(RESOLUTION, dist + 0.5, RESOLUTION):
pose.translation.x = (self.next_pose.position.x -
x) * (d / dist) + x
pose.translation.y = (self.next_pose.position.y -
y) * (d / dist) + y
pose.translation.z = (self.next_pose.position.z -
z) * (d / dist) + z
self.current_pose.multi_dof_joint_state.transforms[
0] = pose # Insert the correct odometry value
validity_msg.robot_state = self.current_pose
# Call service to verify collision
collision_res = self.validity_srv.call(validity_msg)
# print("\nCollision response:")
# print(collision_res)
# Check if robot is in collision
if not collision_res.valid:
# print(validity_msg)
rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format(
pose.translation.x, pose.translation.y,
pose.translation.z))
rospy.logerr('Current pose [x:{} y:{} z:{}]'.format(
x, y, z))
# print(collision_res)
self.move_client.cancel_goal()
self.collision = True
return
class 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 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 RobbieArmActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 5 # Number of joints to manage
self.ERROR_THRESHOLD = 0.15 # Report success if error reaches below threshold
self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME + 'server', anonymous=True)
# Initialize publisher & subscriber for shoulder pan
self.shoulder_pan_frame = 'arm_shoulder_tilt_link'
self.shoulder_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.shoulder_pan_pub = rospy.Publisher('shoulder_pan_controller/command', Float64)
rospy.Subscriber('shoulder_pan_controller/state', JointState, self.read_shoulder_pan)
rospy.wait_for_message('shoulder_pan_controller/state', JointState)
# Initialize publisher & subscriber for arm tilt
self.arm_tilt_frame = 'arm_pan_tilt_bracket'
self.arm_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.arm_tilt_pub = rospy.Publisher('arm_tilt_controller/command', Float64)
rospy.Subscriber('arm_tilt_controller/state', JointState, self.read_arm_tilt)
rospy.wait_for_message('arm_tilt_controller/state', JointState)
# Initialize publisher & subscriber for elbow tilt
self.elbow_tilt_frame = 'arm_bracket'
#self.elbow_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.elbow_tilt_pub = rospy.Publisher('elbow_tilt_controller/command', Float64)
rospy.Subscriber('elbow_tilt_controller/state', JointState, self.read_elbow_tilt)
rospy.wait_for_message('elbow_tilt_controller/state', JointState)
# Initialize publisher & subscriber for wrist pan
self.wrist_pan_frame = 'wrist_pan_link'
self.wrist_pan = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_pan_pub = rospy.Publisher('wrist_pan_controller/command', Float64)
rospy.Subscriber('wrist_pan_controller/state', JointState, self.read_wrist_pan)
rospy.wait_for_message('wrist_pan_controller/state', JointState)
# Initialize publisher & subscriber for wrist tilt
self.wrist_tilt_frame = 'wrist_tilt_link'
self.wrist_tilt = JointState(set_point=0.0, process_value=0.0, error=1.0)
self.wrist_tilt_pub = rospy.Publisher('wrist_tilt_controller/command', Float64)
rospy.Subscriber('wrist_tilt_controller/state', JointState, self.read_wrist_tilt)
rospy.wait_for_message('wrist_tilt_controller/state', JointState)
# Initialize tf listener
self.tf = tf.TransformListener()
# Initialize joints action server
self.result = RobbieArmResult()
self.feedback = RobbieArmFeedback()
self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server = SimpleActionServer(NAME, RobbieArmAction, self.execute_callback)
# Reset arm position
rospy.sleep(1)
self.reset_arm_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def reset_arm_position(self):
# reset arm to cobra position
self.shoulder_pan_pub.publish(0.0)
self.arm_tilt_pub.publish(1.572222)
self.elbow_tilt_pub.publish(-1.572222)
self.wrist_pan_pub.publish(0.0)
self.wrist_tilt_pub.publish(0.0)
rospy.sleep(12)
def read_shoulder_pan(self, pan_data):
self.shoulder_pan = pan_data
self.has_latest_shoulder_pan = True
def read_arm_tilt(self, tilt_data):
self.arm_tilt = tilt_data
self.has_latest_arm_tilt = True
def read_elbow_tilt(self, tilt_data):
self.elbow_tilt = tilt_data
self.has_latest_elbow_tilt = True
def read_wrist_pan(self, rotate_data):
self.wrist_pan = rotate_data
self.has_latest_wrist_pan = True
def read_wrist_tilt(self, rotate_data):
self.wrist_tilt = rotate_data
self.has_latest_wrist_tilt = True
def wait_for_latest_controller_states(self, timeout):
self.has_latest_shoulder_pan = False
self.has_latest_arm_tilt = False
self.has_latest_elbow_tilt = False
self.has_latest_wrist_pan = False
self.has_latest_wrist_tilt = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_shoulder_pan == False or self.has_latest_arm_tilt == False or \
self.has_latest_elbow_tilt == False or self.has_latest_wrist_tilt == False or self.has_latest_wrist_pan == False) and \
(rospy.Time.now() - start < timeout):
r.sleep()
def transform_target_point(self, point):
rospy.loginfo("%s: Retrieving IK solutions", NAME)
rospy.wait_for_service('smart_arm_ik_service', 10)
ik_service = rospy.ServiceProxy('smart_arm_ik_service', SmartArmIK)
resp = ik_service(point)
if (resp and resp.success):
return resp.solutions[0:4]
else:
raise Exception, "Unable to obtain IK solutions."
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
rospy.loginfo("%s: Executing move arm", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
try:
# Convert target point to target joints (find an IK solution)
target_joints = self.transform_target_point(goal.target_point)
except (Exception, tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("%s: Aborted: IK Transform Failure", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal to controllers
self.shoulder_pan_pub.publish(target_joints[0])
self.arm_tilt_pub.publish(target_joints[1])
self.elbow_tilt_pub.publish(target_joints[2])
self.wrist_pan_pub.publish(target_joints[3])
self.wrist_tilt_pub.publish(target_joints[4])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.shoulder_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.arm_tilt.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[2] - self.elbow_tilt.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[3] - self.wrist_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[4] - self.wrist_tilt.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current arm position as feedback
self.feedback.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.arm_position = [self.shoulder_pan.process_value, self.arm_tilt.process_value, \
self.elbow_tilt.process_value, self.wrist_pan.process_value, self.wrist_tilt.process_value]
self.server.set_succeeded(self.result)
class MockNavigation():
def __init__(self, move_base_topic):
self.robot_pose_ = PoseStamped()
self.listener = tf.TransformListener()
self.navigation_succedes = True
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
self.move_base_as_ = SimpleActionServer(
move_base_topic,
MoveBaseAction,
execute_cb = self.move_base_cb,
auto_start = False)
self.move_base_as_.start()
def __del__(self):
self.move_base_as_.__del__()
def move_base_cb(self, goal):
rospy.loginfo('move_base_cb')
self.target_position = goal.target_pose.pose.position
self.target_orientation = goal.target_pose.pose.orientation
self.moves_base = True
while not self.reply:
rospy.sleep(0.2)
(trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
self.robot_pose_.pose.position.x = trans[0]
self.robot_pose_.pose.position.y = trans[1]
feedback = MoveBaseFeedback()
feedback.base_position.pose.position.x = \
self.robot_pose_.pose.position.x
feedback.base_position.pose.position.y = \
self.robot_pose_.pose.position.y
self.move_base_as_.publish_feedback(feedback)
if self.move_base_as_.is_preempt_requested():
self.preempted += 1
rospy.loginfo("Preempted!")
self.moves_base = False
self.move_base_as_.set_preempted()
return None
if self.move_base_as_.is_new_goal_available():
self.preempted += 1
self.move_base_cb(self.move_base_as_.accept_new_goal())
return None
else:
result = MoveBaseResult()
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
if self.navigation_succedes:
self.move_base_as_.set_succeeded(result)
else:
self.move_base_as_.set_aborted(result)
class 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 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 AggressiveGainBuffNode(object):
def __init__(self, name="aggressive_gain_buff_action"):
self.action_name = name
self._as = SimpleActionServer(self.action_name, AggressiveGainBuffAction, execute_cb=self.ExecuteCB, auto_start=False)
self.gain_buff_state = GainBuffState.ROUTE
self.chassis_state_ = 0
self.sub_odom = rospy.Subscriber("aggressive_buff_info", AggressiveGainBuffInfo, callback=self.OdomCB)
self.chassis_mode_client = rospy.ServiceProxy("set_chassis_mode", ChassisMode)
self.chassis_arrive_state = ChassisArriveState()
self.chassis_mode_ = 0
self._ac_navto = SimpleActionClient("nav_to_node_action", NavToAction)
rospy.loginfo('GAIN_BUFF: Connecting NavTo action server...')
ret = self._ac_navto.wait_for_server()
rospy.loginfo('GAIN_BUFF: NavTo sever connected!') if ret else rospy.logerr('error: NavTo server not started!')
self.nav_to_error_code = -1
self.search_start_time = rospy.Time(0)
self._ac_look_n_move = SimpleActionClient("look_n_move_node_action", LookAndMoveAction)
rospy.loginfo('GAIN_BUFF: Connecting Look and Move action server...')
ret = self._ac_look_n_move.wait_for_server(timeout=rospy.Duration(3))
rospy.loginfo('GAIN_BUFF: Look and Move sever connected!') if ret else rospy.logerr('error: Look and Move server not started!')
self.Look_n_move_error_code = -1
self.thread_ = Thread(target=self.Loop)
self.thread_.start()
self._as.start()
def ExecuteCB(self, goal):
print 'GAIN_BUFF:Aggressive gain buff goal recieved!'
route = goal.route_index
CHASSIS_MODE = ChassisModeForm()
self.nav_to_error_code = -1
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
print 'GAIN_BUFF:PREEMPT REQ'
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self._ac_navto.cancel_all_goals()
self._ac_look_n_move.cancel_all_goals()
self._as.set_preempted()
return
if self.gain_buff_state == GainBuffState.ROUTE:
if route == 1 or route == 2:
print 'GAIN_BUFF:Route %i received' % route
if route == 1:
self.SetChassisMode(CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_ONE)
if route == 2:
self.SetChassisMode(CHASSIS_MODE.AGGRESSIVE_GAIN_BUFF_TWO)
print 'GAIN_BUFF:Wait for chassis response...'
chassis_wait_start_time = rospy.get_time()
print 'GAIN_BUFF:chassis state is %i' % (self.chassis_state_)
while self.chassis_state_ == 0 and (rospy.get_time() - chassis_wait_start_time) < CHASSIS_MODE_WAIT_TIME:
continue
if self.chassis_state_ == 1:
print 'GAIN_BUFF:Chassis has responded!'
else:
print 'GAIN_BUFF:Chassis does not respond for the new mode! Return.'
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self._as.set_aborted()
return
chassis_run_start_time = rospy.get_time()
while not (self.chassis_state_ == self.chassis_arrive_state.SUCCEEDED or (rospy.get_time()-chassis_run_start_time > CHASSIS_RUN_WAIT_TIME) ):
if self.chassis_state_ == self.chassis_arrive_state.SUCCEEDED:
# self._as.set_succeeded()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self.gain_buff_state = GainBuffState.SEARCH
self.search_start_time = rospy.get_time()
print 'GAIN_BUFF:Chassis Arrived Buff Area!'
break
else:
print 'GAIN_BUFF:Chassis Aggressive gain buff FAILED, Time out!'
self._as.set_aborted()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
return
elif route == 3:
print 'GAIN_BUFF:Route 3 received'
self.NavToBuff()
if self.nav_to_error_code == 0:
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
# self._as.set_succeeded()
self.gain_buff_state = GainBuffState.SEARCH
self.search_start_time = rospy.get_time()
print 'GAIN_BUFF:Aggressive Gain Buff Route 3 SUCCEED!'
else:
self._as.set_aborted()
print 'No valied route'
return
elif self.gain_buff_state == GainBuffState.SEARCH:
if (rospy.get_time() - self.search_start_time) < SEARCH_BUFF_WAIT_TIME:
self.SearchBuff()
else:
print 'SEARCH BUFF: Chassis Aggressive search buff Time Out!'
self._as.set_aborted()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
self.gain_buff_state = GainBuffState.IDEL
return
else:
print 'Unvaild aggressive gain buff state!'
self._as.set_aborted()
self.SetChassisMode(CHASSIS_MODE.AUTO_SEPARATE_GIMBAL)
return
def Loop(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if self._as.is_preempt_requested():
self._ac_look_n_move.cancel_all_goals()
self._ac_navto.cancel_all_goals()
try:
rate.sleep()
except:
rospy.loginfo('GAIN_BUFF: exiting')
# Set chassis mode to use odom navigation
def SetChassisMode(self, chassis_mode):
if self.chassis_mode_ == chassis_mode:
return
rospy.wait_for_service("set_chassis_mode", timeout=5)
print 'Set chassis mode service connected!'
chassis_mode_msg = ChassisMode()
call_status = self.chassis_mode_client.call(chassis_mode)
chassis_mode_rec_ = ChassisModeResponse()
chassis_mode_rec_ = chassis_mode_msg._response_class
if(call_status and chassis_mode_rec_):
self.chassis_mode_ = chassis_mode
print 'Set Chassis Mode to %i' % chassis_mode
else:
print 'Set gimbal mode failed!'
# return odom arrive result
def OdomCB(self, data):
self.chassis_state_ = data.state
# Call NavTo Action
def NavToBuff(self):
print 'NavTo action is actived!'
g = NavToActionGoal()
for path in BuffPath:
q = tf.transformations.quaternion_from_euler(0,0,path['yaw'])
g.goal.navgoal.pose.position.x = path['x']
g.goal.navgoal.pose.position.y = path['y']
g.goal.navgoal.pose.orientation.z = q[2]
g.goal.navgoal.pose.orientation.w = q[3]
self._ac_navto.send_goal(g.goal, done_cb=self.done_cb, feedback_cb=None)
print 'Waiting for %i point ...' % (path['id'])
self._ac_navto.wait_for_result(timeout=rospy.Duration(10))
if self.nav_to_error_code != 0:
self._ac_navto.cancel_all_goals()
self._as.set_aborted()
print 'AGGRESSIVA_GAIN_BUFF: NavTo Failed!'
return
print 'The result of %i point in BuffPath is arrived!' % path['id']
def SearchBuff(self):
print 'Look and Move action is actived!'
for goal in SearchBuffGoal:
q = tf.transformations.quaternion_from_euler(0.,0.,goal['yaw'])
g = LookAndMoveActionGoal()
g.goal.relative_pose.header.frame_id = "map"
g.goal.relative_pose.pose.position.x = goal['x']
g.goal.relative_pose.pose.position.y = goal['y']
g.goal.relative_pose.pose.orientation.z = q[2]
g.goal.relative_pose.pose.orientation.w = q[3]
self._ac_look_n_move.send_goal(g.goal)
self._ac_look_n_move.wait_for_result(timeout=rospy.Duration(10))
if self.Look_n_move_error_code > 0:
self._ac_look_n_move.cancel_all_goals()
self._as.set_aborted()
print 'AGGRESSIVA_GAIN_BUFF: Look and Move Failed!'
return
# Nav_to done result
def done_cb(self,terminal_state,result):
self.nav_to_error_code = result.error_code
class ObjectDetectionActionServer(object):
_detector_handler = None # type: SingleImageDetectionHandler
_cloud_topic = None # type: str
_filtered_cloud_pub = None # type: rospy.Publisher
_tf_listener = None # type: tf.TransformListener
_target_frame = None # type: str
_cv_bridge = None # type: CvBridge
def __init__(self, action_name, timeout_s=10., **kwargs):
rospy.loginfo('broadcasting action server: ' + action_name)
self._action_server = SimpleActionServer(action_name,
DetectObjectsAction,
execute_cb=self._execute_cb,
auto_start=False)
self._timeout_s = timeout_s
self._initialize(**kwargs)
self._action_server.start()
def _initialize(self, **kwargs):
detection_class = kwargs.get('detection_class', None)
if not issubclass(detection_class, ImageDetectorBase):
raise ValueError(
'"detection_class" is not of ImageDetectorBase type')
class_annotation_file = kwargs.get('class_annotation_file', None)
if not class_annotation_file or not os.path.exists(
class_annotation_file):
raise ValueError('invalid value for "class_annotation_file": ' +
class_annotation_file)
kwargs_file = kwargs.get('kwargs_file', None)
if not kwargs_file or not os.path.exists(kwargs_file):
raise ValueError('invalid value for "kwargs_file": ' + kwargs_file)
# image detection
self._detector_handler = SingleImageDetectionHandler(
detection_class, class_annotation_file, kwargs_file,
'/mas_perception/detection_result')
self._cloud_topic = kwargs.get('cloud_topic', None)
if not self._cloud_topic:
raise ValueError('no cloud topic specified')
self._filtered_cloud_pub = rospy.Publisher('filtered_cloud',
PointCloud2,
queue_size=1)
self._tf_listener = tf.TransformListener()
self._target_frame = kwargs.get('target_frame', '/base_link')
rospy.loginfo('will transform all poses to frame: ' +
self._target_frame)
def _execute_cb(self, _):
# subscribe and wait for cloud message
rospy.loginfo('Object detection action request received')
rospy.loginfo('Waiting for cloud message...')
try:
cloud_msg = rospy.wait_for_message(self._cloud_topic,
PointCloud2,
timeout=self._timeout_s)
except rospy.ROSInterruptException:
# shutdown event interrupts waiting for point cloud
raise
except rospy.ROSException:
rospy.logwarn(
'timeout waiting for cloud topic "{0}" after "{1}" seconds'.
format(self._cloud_topic, self._timeout_s))
self._action_server.set_aborted()
return
rospy.loginfo('Cloud message received; detecting objects...')
img_msg = cloud_msg_to_image_msg(cloud_msg)
original_img_msg = copy.deepcopy(img_msg)
try:
bounding_boxes, classes, confidences = self._detector_handler.process_image_msg(
img_msg)
except RuntimeError as e:
self._action_server.set_aborted(text=e.message)
return
rospy.loginfo('transforming cloud to frame: ' + self._target_frame)
try:
transformed_cloud_msg = transform_cloud_with_listener(
cloud_msg, self._target_frame, self._tf_listener)
except RuntimeError as e:
self._action_server.set_aborted(text=e.message)
return
if self._filtered_cloud_pub.get_num_connections() > 0:
self._filtered_cloud_pub.publish(filtered_cloud)
rospy.loginfo('creating action result and setting success')
result = ObjectDetectionActionServer._get_action_result(
transformed_cloud_msg, bounding_boxes, classes, confidences)
result.image = original_img_msg
self._action_server.set_succeeded(result)
@staticmethod
def _get_action_result(cloud_msg, bounding_boxes, classes, confidences):
"""
:type cloud_msg: PointCloud2
:type bounding_boxes: list
:type classes: list
:type confidences: list
:rtype: DetectObjectsResult
"""
result = DetectObjectsResult()
for index, box in enumerate(bounding_boxes):
detected_obj = get_obj_msg_from_detection(
cloud_msg, box, classes[index], confidences[index],
cloud_msg.header.frame_id)
box_msg = detected_obj.bounding_box
rospy.loginfo(
"Adding object '%s', position (%.3f, %.3f, %.3f), dimensions (%.3f, %.3f, %.3f)"
% (classes[index], box_msg.center.x, box_msg.center.y,
box_msg.center.z, box_msg.dimensions.x,
box_msg.dimensions.y, box_msg.dimensions.z))
result.objects.objects.append(detected_obj)
return result
class FakeTurtle:
def __init__(self):
# define the server, aut-start is set to false so we control when to
# start it
self.server = SimpleActionServer('move_turtle_action',
MoveTurtleAction,
execute_cb=self.action_cb,
auto_start=False)
# deine feedback msg object to be used for publishing feedback
self.feedback_msg = MoveTurtleFeedback()
# deine result msg object to be used for publishing result
self.result_msg = MoveTurtleResult()
self.server.start()
print("action server started ..")
self.rate = rospy.Rate(2)
def turtle_is_up_side_down(self):
# this turtle never goes up side down
# it can always move!
# (this is a dummy function, in real application you might be checking
# your robot status, for example the battery level,
# or motor temperature, etc..)
return False
def action_cb(self, goal):
print("moving turtle to pose: ", goal)
# Write here the logic to control turtle
# this "for" loop is just a demostration, in real application, you
# would be controlling the robot, publish velocity commands, read
# current pose and do some computation..
for _ in range(20):
print('executing goal')
# during execution of the goal, publish feedback msg to the client
self.server.publish_feedback(self.feedback_msg)
# always check if the client has sent a cancel msg. This way the
# task becomes preemtable
if self.server.is_preempt_requested():
# update goal status accordingly
self.server.set_preempted()
print('goal preempted')
# exit from the action_cb method
return None
# check if we need to abort for some reason. Here we abort if
# the node has been killed (ctrl+C) or if the turtle is laying on
# it's back and cannot move anyomre ;)
if rospy.is_shutdown() or self.turtle_is_up_side_down():
# update goal status accordingly
self.server.set_aborted()
print('Turtle cannot move sir! goal aborted')
return None
self.rate.sleep()
# if execution finished, set goal status to SUCCEED and publish the
# result msg
print('done')
self.server.set_succeeded(result=self.result_msg,
text="Mission completed")
class RobbieHeadActionServer():
def __init__(self):
# Initialize constants
self.JOINTS_COUNT = 2 # Number of joints to manage
self.ERROR_THRESHOLD = 0.02 # Report success if error reaches below threshold (0.015 also works)
self.TIMEOUT_THRESHOLD = rospy.Duration(15.0) # Report failure if action does not succeed within timeout threshold
# Initialize new node
rospy.init_node(NAME, anonymous=True)
# Initialize publisher & subscriber for pan
self.head_pan_frame = 'head_pan_link'
self.head_pan = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.head_pan_pub = rospy.Publisher('head_pan_controller/command', Float64)
rospy.Subscriber('head_pan_controller/state_pr2_msgs', JointControllerState, self.read_current_pan)
rospy.wait_for_message('head_pan_controller/state_pr2_msgs', JointControllerState)
# Initialize publisher & subscriber for tilt
self.head_tilt_frame = 'head_tilt_link'
self.head_tilt = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
self.head_tilt_pub = rospy.Publisher('head_tilt_controller/command', Float64)
rospy.Subscriber('head_tilt_controller/state_pr2_msgs', JointControllerState, self.read_current_tilt)
rospy.wait_for_message('head_tilt_controller/state_pr2_msgs', JointControllerState)
# Initialize tf listener
self.tf = tf.TransformListener()
# Initialize point action server
self.result = RobbieHeadResult()
self.feedback = RobbieHeadFeedback()
self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
self.server = SimpleActionServer(NAME, RobbieHeadAction, self.execute_callback, auto_start=False)
# Reset head position
rospy.sleep(1)
self.reset_head_position()
rospy.loginfo("%s: Ready to accept goals", NAME)
def read_current_pan(self, pan_data):
self.head_pan = pan_data
self.has_latest_pan = True
def read_current_tilt(self, tilt_data):
self.head_tilt = tilt_data
self.has_latest_tilt = True
def reset_head_position(self):
self.head_pan_pub.publish(0.0)
self.head_tilt_pub.publish(0.0)
rospy.sleep(5)
def wait_for_latest_controller_states(self, timeout):
self.has_latest_pan = False
self.has_latest_tilt = False
r = rospy.Rate(100)
start = rospy.Time.now()
while (self.has_latest_pan == False or self.has_latest_tilt == False) and (rospy.Time.now() - start < timeout):
r.sleep()
def transform_target_point(self, point):
pan_target_frame = self.head_pan_frame
tilt_target_frame = self.head_tilt_frame
# Wait for tf info (time-out in 5 seconds)
self.tf.waitForTransform(pan_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
self.tf.waitForTransform(tilt_target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(5.0))
# Transform target point to pan reference frame & retrieve the pan angle
pan_target = self.tf.transformPoint(pan_target_frame, point)
pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)
#rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \
# NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle)
# Transform target point to tilt reference frame & retrieve the tilt angle
tilt_target = self.tf.transformPoint(tilt_target_frame, point)
tilt_angle = math.atan2(tilt_target.point.z,
math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))
#rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \
# NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle)
return [pan_angle, tilt_angle]
def execute_callback(self, goal):
r = rospy.Rate(100)
self.result.success = True
self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
rospy.loginfo("%s: Executing move head", NAME)
# Initialize target joints
target_joints = list()
for i in range(self.JOINTS_COUNT):
target_joints.append(0.0)
# Retrieve target joints from goal
if (len(goal.target_joints) > 0):
for i in range(min(len(goal.target_joints), len(target_joints))):
target_joints[i] = goal.target_joints[i]
else:
try:
# Try to convert target point to pan & tilt angles
target_joints = self.transform_target_point(goal.target_point)
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("%s: Aborted: Transform Failure", NAME)
self.result.success = False
self.server.set_aborted()
return
# Publish goal command to controllers
self.head_pan_pub.publish(target_joints[0])
self.head_tilt_pub.publish(target_joints[1])
# Initialize loop variables
start_time = rospy.Time.now()
while (math.fabs(target_joints[0] - self.head_pan.process_value) > self.ERROR_THRESHOLD or \
math.fabs(target_joints[1] - self.head_tilt.process_value) > self.ERROR_THRESHOLD):
# Cancel exe if another goal was received (i.e. preempt requested)
if self.server.is_preempt_requested():
rospy.loginfo("%s: Aborted: Action Preempted", NAME)
self.result.success = False
self.server.set_preempted()
break
# Publish current head position as feedback
self.feedback.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
self.server.publish_feedback(self.feedback)
# Abort if timeout
current_time = rospy.Time.now()
if (current_time - start_time > self.TIMEOUT_THRESHOLD):
rospy.loginfo("%s: Aborted: Action Timeout", NAME)
self.result.success = False
self.server.set_aborted()
break
r.sleep()
if (self.result.success):
rospy.loginfo("%s: Goal Completed", NAME)
self.wait_for_latest_controller_states(rospy.Duration(2.0))
self.result.head_position = [self.head_pan.process_value, self.head_tilt.process_value]
self.server.set_succeeded(self.result)
class V_search(object):
def __init__(self, height, res):
self.resolution = res # Resolution of the motion
self.motion_height = height # Height of the motion to the ground
# Create search server
self.search_server = SimpleActionServer('search_server',
ExecuteSearchAction,
self.searchCallback, False)
self.server_feedback = ExecuteSearchFeedback()
self.server_result = ExecuteSearchResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.next_point = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
## variables
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
self.odometry = None
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback)
# Start trajectory server
self.search_server.start()
def trajectory_feed(self, msg):
'''
Verifies preemption requisitions
'''
if self.search_server.is_preempt_requested():
self.trajectory_client.cancel_goal()
def searchCallback(self, search_area):
'''
Execute a search for vitims into the defined area
'''
x = search_area.x # x size of the area to explore
y = search_area.y # y size of the area to explore
start = search_area.origin # Point to start the search
self.next_point.goal.position.x = start.x # Desired x position
self.next_point.goal.position.y = start.y # Desired y position
theta = 0
# Convert desired angle
q = quaternion_from_euler(0, 0, theta, 'ryxz')
self.next_point.goal.orientation.x = q[0]
self.next_point.goal.orientation.y = q[1]
self.next_point.goal.orientation.z = q[2]
self.next_point.goal.orientation.w = q[3]
x_positions = ceil(x / self.resolution)
y_positions = ceil(y / self.resolution)
x_count = 0 # Counter of steps (in meters traveled)
direction = 1 # Direction of the motion (right, left or up)
trials = 0 # v_search trials
while not rospy.is_shutdown():
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.server_feedback.current_pose = self.odometry
self.odometry_me.release()
if self.search_server.is_preempt_requested():
self.search_server.set_preempted(self.server_result)
return
self.search_server.publish_feedback(self.server_feedback)
self.sonar_me.acquire()
# print("Current height from ground:\n\n{}".format(self.current_height)) # Current distance from ground
h_error = self.motion_height - self.current_height
self.sonar_me.release()
self.odometry_me.acquire()
self.next_point.goal.position.z = self.odometry.position.z + h_error # Desired z position
self.odometry_me.release()
self.trajectory_client.send_goal(self.next_point,
feedback_cb=self.trajectory_feed)
self.trajectory_client.wait_for_result() # Wait for the result
result = self.trajectory_client.get_state(
) # Get the state of the action
# print(result)
if result == GoalStatus.SUCCEEDED:
# Verifies if all the area have been searched
if (self.next_point.goal.position.x == (start.x + x)) and (
(self.next_point.goal.position.y == (start.y + y))):
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.odometry_me.release()
self.search_server.set_succeeded(self.server_result)
return
last_direction = direction
direction = self.square_function(
x_count, 2 * x) # Get the direction of the next step
if last_direction != direction:
# drone moves on y axis
theta = pi / 2
self.next_point.goal.position.y += y / y_positions
elif direction == 1:
# drone moves to the right
theta = 0
self.next_point.goal.position.x += x / x_positions
x_count += x / x_positions
elif direction == -1:
# drone moves to the left
theta = pi
self.next_point.goal.position.x -= x / x_positions
x_count += x / x_positions
# Convert desired angle
q = quaternion_from_euler(0, 0, theta, 'ryxz')
self.next_point.goal.orientation.x = q[0]
self.next_point.goal.orientation.y = q[1]
self.next_point.goal.orientation.z = q[2]
self.next_point.goal.orientation.w = q[3]
elif result == GoalStatus.ABORTED:
trials += 1
if trials == 2:
self.search_server.set_aborted(self.server_result)
return
def square_function(self, x, max_x):
'''
Function to simulate a square wave function
'''
if round(sin(2 * pi * x / max_x), 2) > 0:
return 1
elif round(sin(2 * pi * x / max_x), 2) < 0:
return -1
else:
if round(cos(2 * pi * x / max_x), 2) > 0:
return 1
else:
return -1
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
self.odometry_me.release()