class HeadClient():
def __init__(self):
name_space = '/head_traj_controller/point_head_action'
self.head_client = SimpleActionClient(name_space, PointHeadAction)
self.head_client.wait_for_server()
self.hor_pos = 0.0
self.vert_pos = 0.0
def move_head_vert (self, vert_val):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'torso_lift_link'
head_goal.max_velocity = .3
self.vert_pos = vert_val/50.0 - 1
head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos)
self.head_client.send_goal(head_goal)
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def move_head_hor (self, hor_val):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'torso_lift_link'
head_goal.max_velocity = .3
self.hor_pos = -(hor_val/20.0 - 2.5)
head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos)
self.head_client.send_goal(head_goal)
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
class ActionTasks:
def __init__(self, script_path):
rospy.init_node('action_testr')
rospy.on_shutdown(self.cleanup)
self.fridge = (Pose(Point(0, 0, 0.0), Quaternion(0.0, 0.0, 0, 1)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
self.move_to(self.fridge)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
#self.client.wait_for_result()
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
rospy.loginfo("Shutting down talk node...")
def test_grasp_action(self):
"""Test sending a grasp."""
goal = GraspGoal()
goal.grasp = self.mk_grasp({
'LFJ3': 1.4, 'RFJ3': 1.4, 'MFJ3': 1.4, 'FFJ3': 1.4,
'LFJ0': 3.0, 'RFJ0': 3.0, 'MFJ0': 3.0, 'FFJ0': 3.0,
})
# Get a action client
client = SimpleActionClient('grasp', GraspAction)
client.wait_for_server()
# Send pre-grasp
goal.pre_grasp = True
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(20.0))
self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED,
"Action did not return in SUCCEEDED state.")
rospy.sleep(2)
# Send grasp
goal.pre_grasp = False
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(20.0))
self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED,
"Action did not return in SUCCEEDED state.")
class ActionTasks:
def __init__(self, script_path):
rospy.init_node('action_testr')
rospy.on_shutdown(self.cleanup)
self.fridge = (Pose(Point(0.295, -2.304, 0.0), Quaternion(0.0, 0.0, -0.715, 0.699)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
self.move_to(self.fridge)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
#self.client.wait_for_result()
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
rospy.loginfo("Shutting down talk node...")
class Tiago:
def __init__(self):
rospy.init_node('run_motion_python')
rospy.loginfo("Starting run_motion_python application...")
wait_for_valid_time(10.0)
self.client = SimpleActionClient('/play_motion', PlayMotionAction)
rospy.loginfo("Waiting for Action Server...")
self.client.wait_for_server()
def act(self, action):
goal = PlayMotionGoal()
goal.motion_name = action
goal.skip_planning = False
goal.priority = 0 # Optional
rospy.loginfo("Sending goal with motion: " + sys.argv[1])
self.client.send_goal(goal)
rospy.loginfo("Waiting for result...")
action_ok = self.client.wait_for_result(rospy.Duration(30.0))
state = self.client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " +
str(get_status_string(state)))
else:
rospy.logwarn("Action failed with state: " +
str(get_status_string(state)))
def reset(self):
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = False
goal.priority = 0 # Optional
rospy.loginfo("Sending goal with motion: " + sys.argv[1])
self.client.send_goal(goal)
rospy.loginfo("Waiting for result...")
action_ok = self.client.wait_for_result(rospy.Duration(30.0))
state = self.client.get_state()
if action_ok:
rospy.loginfo("Action finished succesfully with state: " +
str(get_status_string(state)))
else:
rospy.logwarn("Action failed with state: " +
str(get_status_string(state)))
class TestClientsAndServer(unittest.TestCase):
def setUp(self):
rospy.init_node('test_move_end_effector_server_node')
self.effector_client = Client(move_end_effector_controller_topic,
MoveEndEffectorAction)
self.server = MoveEndEffectorServer()
self.goal_maker = EffectorGoalMaker()
self.sensor_pub = Publisher('mock/sensor', String, queue_size=1)
self.linear_actuator_pub = Publisher('mock/linear_actuator',
String,
queue_size=1)
self.head_pub = Publisher('mock/head', String, queue_size=1)
rospy.sleep(1)
def test_execute_cb(self):
''' Tests if server is set correctly (triggered by callback!) '''
self.sensor_pub.publish('success:1')
self.linear_actuator_pub.publish('success:1')
self.head_pub.publish('success:1')
self.goal = MoveEndEffectorGoal()
self.goal = self.goal_maker.create_goal()
self.server.wait_for_servers()
self.effector_client.send_goal(self.goal)
self.effector_client.wait_for_result()
self.assertEqual(self.effector_client.get_state(),
GoalStatus.SUCCEEDED)
self.sensor_pub.publish('abort:1')
self.linear_actuator_pub.publish('success:1')
self.head_pub.publish('success:1')
self.goal = MoveEndEffectorGoal()
self.goal = self.goal_maker.create_goal()
self.effector_client.send_goal(self.goal)
self.effector_client.wait_for_result()
self.assertEqual(self.effector_client.get_state(), GoalStatus.ABORTED)
self.sensor_pub.publish('preempt:1')
self.linear_actuator_pub.publish('success:1')
self.head_pub.publish('preempt:1')
self.goal = MoveEndEffectorGoal()
self.goal = self.goal_maker.create_goal()
self.effector_client.send_goal(self.goal)
self.effector_client.wait_for_result()
self.assertEqual(self.effector_client.get_state(),
GoalStatus.PREEMPTED)
self.sensor_pub.publish('abort:1')
self.linear_actuator_pub.publish('success:1')
self.head_pub.publish('preempt:1')
self.goal = MoveEndEffectorGoal()
self.goal = self.goal_maker.create_goal()
self.effector_client.send_goal(self.goal)
self.effector_client.wait_for_result()
self.assertEqual(self.effector_client.get_state(), GoalStatus.ABORTED)
class plan_task(smach.State):
def __init__(self, mode=PlanGoal.NORMAL):
smach.State.__init__(
self,
outcomes=['success', 'failure'],
input_keys=['domain_file', 'problem_file', 'planner'],
output_keys=['plan'])
self.mode = mode
self.planner_client = SimpleActionClient(
'/mir_task_planning/task_planner_server/plan_task', PlanAction)
self.planner_client.wait_for_server()
rospy.sleep(0.2)
def execute(self, userdata):
goal = PlanGoal(domain_file=userdata.domain_file,
problem_file=userdata.problem_file,
planner=userdata.planner,
mode=self.mode)
self.planner_client.send_goal(goal)
self.planner_client.wait_for_result()
state = self.planner_client.get_state()
result = self.planner_client.get_result()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Found a plan with %i actions",
len(result.plan.plan))
userdata.plan = result.plan
return 'success'
elif state == GoalStatus.ABORTED:
rospy.logerr("Failed to plan")
rospy.sleep(2.0)
return 'failure'
def move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()):
move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction)
move_arm_client.wait_for_server()
rospy.loginfo('connected to move_left_arm action server')
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = 'left_arm'
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)]
goal.motion_plan_request.allowed_contacts = allowed_contacts
goal.motion_plan_request.link_padding = link_padding
goal.motion_plan_request.ordered_collision_operations = collision_operations
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.loginfo('timed out trying to achieve a joint goal')
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo('action finished: %s' % str(state))
return True
else:
rospy.loginfo('action failed: %s' % str(state))
return False
def head_action(self, x, y, z, wait=False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(
rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo(
"Head movement goal failed with error code: " +
str(self.goal_states[state]))
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation', GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value))
result = self.client.get_state()
if result == GoalStatus.PREEMPTED:
return
if result == GoalStatus.SUCCEEDED:
self._ps.set_succeeded()
else:
self._ps.set_aborted()
def preempt_cb(self):
self.client.cancel_all_goals()
self._ps.set_preempted()
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 InitMove(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded', 'supervise'])
self.move_base_client = SimpleActionClient('move_base', MoveBaseAction)
def execute(self, ud):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
pose = PoseStamped()
pose.header.frame_id = 'map'
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = 2.0
pose.pose.position.y = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
goal.target_pose = pose
goal.target_pose.header.stamp = rospy.Time.now()
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result(rospy.Duration(30))
status = self.move_base_client.get_state()
if status == GoalStatus.SUCCEEDED:
return 'succeeded'
else:
return 'supervise'
class unstage_object(smach.State):
def __init__(self, platform=None):
smach.State.__init__(self,
outcomes=['success', 'failed'],
input_keys=['goal'])
self.client = SimpleActionClient('unstage_object_server',
GenericExecuteAction)
self.client.wait_for_server()
self.goal = GenericExecuteGoal()
if platform is not None:
self.goal.parameters.append(
KeyValue(key='platform', value=str(platform).upper()))
def execute(self, userdata):
# initialise platform in goal if not already initialised
current_platform = Utils.get_value_of(self.goal.parameters, 'platform')
if current_platform is None:
platform = Utils.get_value_of(userdata.goal.parameters, 'platform')
if platform is None:
rospy.logwarn('Platform not provided. Using default')
platform = 'platform_middle'
self.goal.parameters.append(
KeyValue(key='platform', value=str(platform).upper()))
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(30.0))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return 'success'
else:
return 'failed'
class ArmController:
def __init__(self, arm):
self.client = SimpleActionClient(
"%s_arm_controller/joint_trajectory_action" % arm,
JointTrajectoryAction)
#wait for the action servers to come up
rospy.loginfo("[ARM] Waiting for %s controller" % arm)
self.client.wait_for_server()
rospy.loginfo("[ARM] Got %s controller" % arm)
def start_trajectory(self, trajectory, set_time_stamp=True, wait=True):
"""Creates an action from the trajectory and sends it to the server"""
goal = JointTrajectoryGoal()
goal.trajectory = trajectory
if set_time_stamp:
goal.trajectory.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
if wait:
self.wait()
def wait(self):
self.client.wait_for_result()
def is_done(self):
return self.client.get_state() > SimpleGoalState.ACTIVE
class DoReset(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['reset_plan'])
self.eef_pub = rospy.Publisher('/CModelRobotOutput', eef_cmd, queue_size=1, latch=True)
self.move_arm_client = SimpleActionClient('/wm_arm_driver_node/execute_plan', executePlanAction)
def execute(self, ud):
rospy.logdebug("Entered 'RO_RESET' state.")
hand_cmd = eef_cmd()
hand_cmd.rACT = 1 # activate gripper
hand_cmd.rGTO = 1 # request to go to position
hand_cmd.rSP = 200 # set activation speed (0[slowest]-255[fastest])
hand_cmd.rFR = 0 # set force limit (0[min] - 255[max])
hand_cmd.rPR = 0 # request to open
self.eef_pub.publish(hand_cmd)
self.move_arm_client.wait_for_server()
goal = executePlanGoal()
goal.trajectory = ud.reset_plan
self.move_arm_client.send_goal(goal)
self.move_arm_client.wait_for_result()
status = self.move_arm_client.get_state()
if status == GoalStatus.SUCCEEDED:
return 'succeeded'
elif status == GoalStatus.PREEMPTED:
return 'preempted'
else:
return 'aborted'
class ExplorationController:
def __init__(self):
self._plan_service = rospy.ServiceProxy('get_exploration_path', GetRobotTrajectory)
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
def run(self):
r = rospy.Rate(1 / 7.0)
while not rospy.is_shutdown():
self.run_once()
r.sleep()
def run_once(self):
path = self._plan_service().trajectory
poses = path.poses
if not path.poses:
rospy.loginfo('No frontiers left.')
return
rospy.loginfo('Moving to frontier...')
self.move_to_pose(poses[-1])
def move_to_pose(self, pose_stamped, timeout=20.0):
goal = MoveBaseGoal()
goal.target_pose = pose_stamped
self._move_base.send_goal(goal)
self._move_base.wait_for_result(rospy.Duration(timeout))
return self._move_base.get_state() == GoalStatus.SUCCEEDED
class ExplorationController:
def __init__(self):
rospy.wait_for_service('get_exploration_path', timeout=2)
self._plan_service = rospy.ServiceProxy('get_exploration_path',
GetRobotTrajectory)
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
def run(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
self.run_once()
r.sleep()
def run_once(self):
global count
if (count >= 20
or self._move_base.get_state() == GoalStatus.SUCCEEDED):
path = self._plan_service().trajectory
poses = path.poses
if not path.poses:
rospy.loginfo('No frontiers left.')
return
rospy.loginfo('Moving to frontier...')
self.move_to_pose(poses[-1])
count = 0
else:
count += 1
def move_to_pose(self, pose_stamped, timeout=20.0):
goal = MoveBaseGoal()
goal.target_pose = pose_stamped
self._move_base.send_goal(goal)
class Move(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['wp_reached', 'wp_not_reached', 'test_finished'],
input_keys=['move_waypoints', 'move_target_wp', 'move_wp_str'],
output_keys=['move_target_wp'])
self.move_base_client = SimpleActionClient('move_base', MoveBaseAction)
self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True)
def execute(self, ud):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = ud.move_waypoints[ud.move_target_wp]
goal.target_pose.header.stamp = rospy.Time.now()
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result()
status = self.move_base_client.get_state()
tts_msg = String()
if status == GoalStatus.SUCCEEDED:
tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "."
self.tts_pub.publish(tts_msg)
if ud.move_target_wp == 0:
return 'test_finished'
else:
ud.move_target_wp -= 1
return 'succeeded'
else:
return 'wp_not_reached'
def _exec_action(action_client: SimpleActionClient, goal, stopping_event: EventBase = None):
"""Executes goal with specified action client and optional event that stops action execution when triggered.
This is a private method.
:param action_client: Action client to use.
:param goal: Action goal to execute.
:param stopping_event: Optional event that stops goal execution when triggered.
:return: None if stopping_event is None. True if goal execution is stopped because of triggered stopping_event,
False otherwise.
"""
if stopping_event is None:
action_client.send_goal(goal)
action_client.wait_for_result()
return
stopping_event.start_listening()
action_client.send_goal(goal,
active_cb=None,
feedback_cb=None,
done_cb=None)
check_rate = rospy.Rate(100)
while True:
goal_state = action_client.get_state()
if goal_state == SimpleGoalState.DONE:
stopping_event.stop_listening()
return False
if stopping_event.is_triggered():
action_client.cancel_goal()
return True
check_rate.sleep()
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation',
GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value))
result = self.client.get_state()
if result == GoalStatus.PREEMPTED:
return
if result == GoalStatus.SUCCEEDED:
self._ps.set_succeeded()
else:
self._ps.set_aborted()
def preempt_cb(self):
self.client.cancel_all_goals()
self._ps.set_preempted()
class RecordPoseStampedFromPlayMotion():
"""Manage the learning from a play motion gesture"""
def __init__(self):
rospy.loginfo("Initializing RecordFromPlayMotion")
rospy.loginfo("Connecting to AS: '" + PLAY_MOTION_AS + "'")
self.play_motion_as = SimpleActionClient(PLAY_MOTION_AS,
PlayMotionAction)
self.play_motion_as.wait_for_server()
rospy.loginfo("Connected.")
self.current_rosbag_name = "uninitialized_rosbag_name"
self.lfee = LearnFromEndEffector(['/tf_to_ps'], ['right_arm'])
def play_and_record(self,
motion_name,
groups=['right_arm'],
bag_name="no_bag_name_set"):
"""Play the specified motion and start recording poses.
Try to get the joints to record from the metadata of the play_motion gesture
or, optionally, specify the joints to track"""
# Check if motion exists in param server
PLAYMOTIONPATH = '/play_motion/motions/'
if not rospy.has_param(PLAYMOTIONPATH + motion_name):
rospy.logerr("Motion named: " + motion_name +
" does not exist in param server at " +
PLAYMOTIONPATH + motion_name)
return
else:
rospy.loginfo("Found motion " + motion_name +
" in param server at " + PLAYMOTIONPATH +
motion_name)
# Get it's info
motion_info = rospy.get_param(PLAYMOTIONPATH + motion_name)
# check if joints was specified, if not, get the joints to actually save
if len(groups) > 0:
joints_to_record = groups
else:
joints_to_record = motion_info['groups']
rospy.loginfo("Got groups: " + str(joints_to_record))
# play motion
rospy.loginfo("Playing motion!")
pm_goal = PlayMotionGoal(motion_name, False, 0)
self.play_motion_as.send_goal(pm_goal)
self.lfee.start_learn(motion_name, bag_name)
done_with_motion = False
while not done_with_motion:
state = self.play_motion_as.get_state()
#rospy.loginfo("State is: " + str(state) + " which is: " + goal_status_dict[state])
if state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED:
done_with_motion = True
elif state != GoalStatus.PENDING and state != GoalStatus.ACTIVE:
rospy.logerr("We got state " + str(state) +
" unexpectedly, motion failed. Aborting.")
return None
rospy.sleep(0.1)
# data is written to rosbag in here
motion_data = self.lfee.stop_learn()
return motion_data
def testsimple(self):
client = SimpleActionClient('reference_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(10.0)),
'Could not connect to the action server')
goal = TestGoal(1)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
self.assertEqual("The ref server has succeeded", client.get_goal_status_text())
goal = TestGoal(2)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.ABORTED, client.get_state())
self.assertEqual("The ref server has aborted", client.get_goal_status_text())
def testsimple(self):
client = SimpleActionClient('reference_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(10.0)),
'Could not connect to the action server')
goal = TestGoal(1)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
self.assertEqual("The ref server has succeeded", client.get_goal_status_text())
goal = TestGoal(2)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.ABORTED, client.get_state())
self.assertEqual("The ref server has aborted", client.get_goal_status_text())
class LinearActuatorClient(object):
"""docstring for LinearActuatorClient"""
def __init__(self):
self._name = "LinearActuatorClient"
self.goal = linear_actuator_client['goal']
self.topic = linear_actuator_client['topic']
self.action = linear_actuator_client['action']
self.command_dict = translate_command['to_linear_actuator']
self.client = Client(self.topic, self.action)
def get_name(self):
return self._name
def fill_goal(self, effector_goal):
self.goal.command = self.command_dict[effector_goal.command]
self.goal.point_of_interest = effector_goal.point_of_interest
self.goal.center_point = effector_goal.center_point
def send_goal(self):
self.client.send_goal(self.goal)
def wait_server(self):
self.client.wait_for_server()
def wait_result(self):
self.client.wait_for_result()
def has_succeeded(self):
return self.client.get_state() == GoalStatus.SUCCEEDED
def has_aborted(self):
return self.client.get_state() == GoalStatus.ABORTED
def has_preempted(self):
return self.client.get_state() == GoalStatus.PREEMPTED
def has_been_recalled(self):
return self.client.get_state() == GoalStatus.RECALLED
def preempt_if_active(self):
if self.client.get_state() == GoalStatus.ACTIVE:
self.client.cancel_all_goals()
def run_grasp(grasp, pre=False):
goal = GraspGoal()
goal.grasp = grasp
goal.pre_grasp = pre
client = SimpleActionClient('grasp', GraspAction)
client.wait_for_server()
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(20.0))
if client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("Success!")
else:
rospy.logerr("Fail!")
class AsyncSimpleActionClient:
def __init__(self, ns, action_spec):
self._client = SimpleActionClient(ns, action_spec)
self._client.wait_for_server()
async def send_goal(self, goal):
self._client.send_goal(goal)
loop = asyncio.get_event_loop()
await loop.run_in_executor(None, self._client.wait_for_result)
return self._client.get_state(), self._client.get_result()
class safe_land(object):
'''
Allows a human to teleoperate the robot through a joystick.
'''
def __init__(self, name):
self.robot_name = name
self.state = 'IDLE'
# Landing service
self._land_client = SimpleActionClient(
"safe_land_server",
ExecuteLandAction) # Create a client to the v_search server
self._land_client.wait_for_server() # Wait server to be ready
self._land_goal = ExecuteLandGoal() # Message to send the goal region
self.pub = rospy.Publisher("/{}/maneuvers/out".format(name),
events_message,
queue_size=10) # Publisher object
self.msg = events_message() # Message object
def execute(self):
rospy.loginfo("Starting Safe Land!")
self.state = 'EXE'
# Start safe_land
self._land_client.send_goal(self._land_goal)
self._land_client.wait_for_result()
state = self._land_client.get_state() # Get the state of the action
print(state)
if state == GoalStatus.SUCCEEDED:
# safe_land successfully executed
self.state = 'IDLE' # Set IDLE state
self.msg.event = 'end_safe_land'
self.pub.publish(
self.msg
) # Send the message signaling that the safe_land is complete
else:
# The server aborted the motion
self.state = 'ERROR' # Set ERROR state
self.msg.event = 'safe_land_error'
self.pub.publish(self.msg) # Send message signaling the error
def reset(self):
rospy.loginfo("Reseting Safe Land!")
self.state = 'IDLE'
def erro(self):
rospy.loginfo("Safe Land Erro!")
self.state = 'ERROR' # Set ERROR state
self._land_client.cancel_goal() # Cancel the motion of the robot
def test_one(self):
client = SimpleActionClient('reference_simple_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(2.0)),
'Could not connect to the action server')
goal = TestGoal(1)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(2.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
goal = TestGoal(2)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.ABORTED, client.get_state())
goal = TestGoal(3)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
#The simple server can't reject goals
self.assertEqual(GoalStatus.ABORTED, client.get_state())
goal = TestGoal(9)
saved_feedback={};
def on_feedback(fb):
rospy.loginfo("Got feedback")
saved_feedback[0]=fb
client.send_goal(goal,feedback_cb=on_feedback)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
self.assertEqual(saved_feedback[0].feedback,9)
def test_one(self):
client = SimpleActionClient('reference_simple_action', TestAction)
self.assert_(client.wait_for_server(rospy.Duration(2.0)),
'Could not connect to the action server')
goal = TestGoal(1)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(2.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
goal = TestGoal(2)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.ABORTED, client.get_state())
goal = TestGoal(3)
client.send_goal(goal)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
#The simple server can't reject goals
self.assertEqual(GoalStatus.ABORTED, client.get_state())
goal = TestGoal(9)
saved_feedback = {}
def on_feedback(fb):
rospy.loginfo("Got feedback")
saved_feedback[0] = fb
client.send_goal(goal, feedback_cb=on_feedback)
self.assert_(client.wait_for_result(rospy.Duration(10.0)),
"Goal didn't finish")
self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
self.assertEqual(saved_feedback[0].feedback, 9)
class RecordPoseStampedFromPlayMotion():
"""Manage the learning from a play motion gesture"""
def __init__(self):
rospy.loginfo("Initializing RecordFromPlayMotion")
rospy.loginfo("Connecting to AS: '" + PLAY_MOTION_AS + "'")
self.play_motion_as = SimpleActionClient(PLAY_MOTION_AS, PlayMotionAction)
self.play_motion_as.wait_for_server()
rospy.loginfo("Connected.")
self.current_rosbag_name = "uninitialized_rosbag_name"
self.lfee = LearnFromEndEffector(['/tf_to_ps'], ['right_arm'])
def play_and_record(self, motion_name, groups=['right_arm'], bag_name="no_bag_name_set"):
"""Play the specified motion and start recording poses.
Try to get the joints to record from the metadata of the play_motion gesture
or, optionally, specify the joints to track"""
# Check if motion exists in param server
PLAYMOTIONPATH = '/play_motion/motions/'
if not rospy.has_param(PLAYMOTIONPATH + motion_name):
rospy.logerr("Motion named: " + motion_name + " does not exist in param server at " + PLAYMOTIONPATH + motion_name)
return
else:
rospy.loginfo("Found motion " + motion_name + " in param server at " + PLAYMOTIONPATH + motion_name)
# Get it's info
motion_info = rospy.get_param(PLAYMOTIONPATH + motion_name)
# check if joints was specified, if not, get the joints to actually save
if len(groups) > 0:
joints_to_record = groups
else:
joints_to_record = motion_info['groups']
rospy.loginfo("Got groups: " + str(joints_to_record))
# play motion
rospy.loginfo("Playing motion!")
pm_goal = PlayMotionGoal(motion_name, False, 0)
self.play_motion_as.send_goal(pm_goal)
self.lfee.start_learn(motion_name, bag_name)
done_with_motion = False
while not done_with_motion:
state = self.play_motion_as.get_state()
#rospy.loginfo("State is: " + str(state) + " which is: " + goal_status_dict[state])
if state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED:
done_with_motion = True
elif state != GoalStatus.PENDING and state != GoalStatus.ACTIVE:
rospy.logerr("We got state " + str(state) + " unexpectedly, motion failed. Aborting.")
return None
rospy.sleep(0.1)
# data is written to rosbag in here
motion_data = self.lfee.stop_learn()
return motion_data
class perceive_cavity(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=["success", "failed"])
self.client = SimpleActionClient("perceive_cavity_server", GenericExecuteAction)
self.client.wait_for_server()
self.goal = GenericExecuteGoal()
def execute(self, userdata):
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(30.0))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return "success"
else:
return "failed"
def look_at_r_gripper(self):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "r_gripper_tool_frame"
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = Point(0, 0, 0)
head_goal.max_velocity = 1.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def look_at_r_gripper(self):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "r_gripper_tool_frame"
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = Point(0, 0, 0)
head_goal.max_velocity = 1.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
class pick_object(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=["success", "failed"])
self.client = SimpleActionClient("wbc_pick_object_server", GenericExecuteAction)
self.client.wait_for_server()
self.goal = GenericExecuteGoal()
self.goal.parameters.append(KeyValue(key="object", value="any"))
def execute(self, userdata):
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(30.0))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return "success"
else:
return "failed"
class perceive_location(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['success', 'failed'])
self.client = SimpleActionClient('perceive_location_server',
GenericExecuteAction)
self.client.wait_for_server()
self.goal = GenericExecuteGoal()
def execute(self, userdata):
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(30.0))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return 'success'
else:
return 'failed'
class GUI(object):
""" Communication with the operator."""
def __init__(self, verbose=False):
self.verbose = verbose
self.gui_result = None
self.client = Client(topics.gui_validation, ValidateVictimGUIAction)
def cancel_all_goals(self):
log.debug('Waiting for the GUI action server...')
self.client.wait_for_server()
log.info('Canceling all goals on GUI.')
self.client.cancel_all_goals()
sleep(3)
def send_request(self, target):
""" Sends a validation request to the robot operator.
:param :target A target to be validated.
"""
goal = ValidateVictimGUIGoal()
goal.victimId = target.id
goal.victimFoundx = target.victimPose.pose.position.x
goal.victimFoundy = target.victimPose.pose.position.y
goal.probability = target.probability
goal.sensorIDsFound = target.sensors
log.debug('Waiting for the GUI action server.')
self.client.wait_for_server()
log.info('Sending validation request.')
if self.verbose:
log.debug(target)
self.client.send_goal(goal)
log.info('Waiting for response.')
self.client.wait_for_result()
status = self.client.get_state()
verbose_status = ACTION_STATES[status]
if status == GoalStatus.SUCCEEDED:
log.info('Validation request succeded!')
self.gui_result = self.client.get_result()
return True
else:
log.error('Validation request failed with %s.', verbose_status)
return False
def result(self):
return self.gui_result
class HeadNode():
def __init__(self):
name_space = '/head_traj_controller/point_head_action'
self.head_client = SimpleActionClient(name_space, PointHeadAction)
self.head_client.wait_for_server()
def moveHead(self, theta, phi):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(math.cos(theta), math.sin(theta), phi)
self.head_client.send_goal(head_goal)
self.head_client.wait_for_result(rospy.Duration(10.0))
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def movebase_client(self, x, y, qz, qw):
client = SimpleActionClient("move_base", MoveBaseAction)
client.wait_for_server(rospy.Duration(3))
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position = Point(x, y, 0)
goal.target_pose.pose.orientation = Quaternion(0, 0, qz, qw)
client.send_goal(goal)
wait = client.wait_for_result()
if not wait:
rospy.logerr("Action Server NOT Available!")
rospy.signal_shutdown("Action Server NOT Available!")
else:
return client.get_state()
class move_base(smach.State):
def __init__(self, destination_location):
smach.State.__init__(self,
outcomes=["success", "failed"],
input_keys=["goal"])
self.client = SimpleActionClient("move_base_safe_server",
GenericExecuteAction)
self.client.wait_for_server()
self.goal = GenericExecuteGoal()
self.destination_location = destination_location
def execute(self, userdata):
self.goal.parameters = []
self.goal.parameters.append(
KeyValue(key="arm_safe_position", value="barrier_tape"))
# if location is sent as an argument to this state, set it here
if self.destination_location is not None:
self.goal.parameters.append(
KeyValue(key="destination_location",
value=str(self.destination_location).upper()))
current_destination_location = Utils.get_value_of(
self.goal.parameters, "destination_location")
# if location has not been set read it from userdata (either
# destination_location or location)
if current_destination_location is None:
location = Utils.get_value_of(userdata.goal.parameters,
"destination_location")
if location is None:
location = Utils.get_value_of(userdata.goal.parameters,
"location")
if location is None:
rospy.logerr(
"Location not specified. Not calling move_base")
return 'failed'
self.goal.parameters.append(
KeyValue(key="destination_location",
value=str(location).upper()))
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(int(15.0)))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return "success"
else:
return "failed"
def move_head():
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = self.get_frame()
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = self.get_target()
head_goal.max_velocity = 10.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(2.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
self.gui.show_text_in_rviz("Head!")
class perceive_cavity(smach.State):
def __init__(self, perception_mode='three_view'):
smach.State.__init__(self, outcomes=["success", "failed"])
self.client = SimpleActionClient("perceive_cavity_server",
GenericExecuteAction)
self.client.wait_for_server()
self.goal = GenericExecuteGoal()
self.goal.parameters.append(
KeyValue(key='perception_mode', value=perception_mode))
def execute(self, userdata):
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(30.0))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return "success"
else:
return "failed"
class place_object(smach.State):
def __init__(self, platform_name):
smach.State.__init__(self, outcomes=['success', 'failed'])
self.client = SimpleActionClient('place_object_server',
GenericExecuteAction)
self.client.wait_for_server()
self.goal = GenericExecuteGoal()
self.goal.parameters.append(
KeyValue(key='location', value=str(platform_name).upper()))
def execute(self, userdata):
self.client.send_goal(self.goal)
self.client.wait_for_result(rospy.Duration.from_sec(15.0))
state = self.client.get_state()
if state == GoalStatus.SUCCEEDED:
return 'success'
else:
return 'failed'
def move_head():
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = self.get_frame()
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = self.get_target()
head_goal.max_velocity = 10.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(2.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
self.gui.show_text_in_rviz("Head!")
def tilt_head(self, down=True):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = self.get_frame()
head_goal.min_duration = rospy.Duration(0.3)
if down:
head_goal.target.point = Point(1, 0, Head.speed * -0.1)
else:
head_goal.target.point = Point(1, 0, Head.speed * 0.1)
head_goal.max_velocity = 10.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(2.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def main():
rospy.init_node("simple_navigation_goals")
move_base_client = SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo('Connecting to server')
move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'komodo_1/base_link'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = -1.0
goal.target_pose.pose.orientation.w = 1.0
rospy.loginfo('Sending goal')
move_base_client.send_goal(goal)
move_base_client.wait_for_result()
if move_base_client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Hooray, the base moved 1 meter forward')
else:
rospy.loginfo('The base failed to move forward 1 meter for some reason')
def head_action(self, x, y, z, wait = False):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(x, y, z)
head_client.send_goal(head_goal)
if wait:
# wait for the head movement to finish before we try to detect and pickup an object
finished_within_time = head_client.wait_for_result(rospy.Duration(5))
# Check for success or failure
if not finished_within_time:
head_client.cancel_goal()
rospy.loginfo("Timed out achieving head movement goal")
else:
state = head_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Head movement goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Head movement goal failed with error code: " + str(self.goal_states[state]))
class SocialGaze:
def __init__(self):
self.defaultLookatPoint = Point(1,0,1.35)
self.downLookatPoint = Point(0.5,0,0.5)
self.targetLookatPoint = Point(1,0,1.35)
self.currentLookatPoint = Point(1,0,1.35)
self.currentGazeAction = GazeGoal.LOOK_FORWARD;
self.prevGazeAction = self.currentGazeAction
self.prevTargetLookatPoint = array(self.defaultLookatPoint);
# Some constants
self.doNotInterrupt = [GazeGoal.GLANCE_RIGHT_EE, GazeGoal.GLANCE_LEFT_EE, GazeGoal.NOD, GazeGoal.SHAKE];
self.nodPositions = [Point(1,0,1.05), Point(1,0,1.55)]
self.shakePositions = [Point(1,0.2,1.35), Point(1,-0.2,1.35)]
self.nNods = 5
self.nShakes = 5
self.nodCounter = 5
self.shakeCounter = 5
self.facePos = None
## Action client for sending commands to the head.
self.headActionClient = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
self.headActionClient.wait_for_server()
rospy.loginfo('Head action client has responded.')
self.headGoal = PointHeadGoal()
self.headGoal.target.header.frame_id = 'base_link'
self.headGoal.min_duration = rospy.Duration(1.0)
self.headGoal.target.point = Point(1,0,1)
## Client for receiving detected faces
#self.faceClient = SimpleActionClient('face_detector_action', FaceDetectorAction)
## Service client for arm states
self.tfListener = TransformListener()
## Server for gaze requested gaze actions
self.gazeActionServer = actionlib.SimpleActionServer('gaze_action', GazeAction, self.executeGazeAction, False)
self.gazeActionServer.start()
## Callback function to receive ee states and face location
def getEEPos(self, armIndex):
fromFrame = '/base_link'
if (armIndex == 0):
toFrame = '/r_wrist_roll_link'
else:
toFrame = '/l_wrist_roll_link'
try:
t = self.tfListener.getLatestCommonTime(fromFrame, toFrame)
(position, rotation) = self.tfListener.lookupTransform(fromFrame, toFrame, t)
except:
rospy.logerr('Could not get the end-effector pose.')
#objPoseStamped = PoseStamped()
#objPoseStamped.header.stamp = t
#objPoseStamped.header.frame_id = '/base_link'
#objPoseStamped.pose = Pose()
#relEEPose = self.tfListener.transformPose(toFrame, objPoseStamped)
return Point(position[0], position[1], position[2])
def getFaceLocation(self):
connected = self.faceClient.wait_for_server(rospy.Duration(1.0))
if connected:
fgoal = FaceDetectorGoal()
self.faceClient.send_goal(fgoal)
self.faceClient.wait_for_result()
f = self.faceClient.get_result()
## If there is one face follow that one, if there are more than one, follow the closest one
closest = -1
if len(f.face_positions) == 1:
closest = 0
elif len(f.face_positions) > 0:
closest_dist = 1000
for i in range(len(f.face_positions)):
dist = f.face_positions[i].pos.x*f.face_positions[i].pos.x + f.face_positions[i].pos.y*f.face_positions[i].pos.y + f.face_positions[i].pos.z*f.face_positions[i].pos.z
if dist < closest_dist:
closest = i
closest_dist = dist
if closest > -1:
self.facePos = array([f.face_positions[closest].pos.x, f.face_positions[closest].pos.y, f.face_positions[closest].pos.z])
else:
rospy.logwarn('No faces were detected.')
self.facePos = self.defaultLookatPoint
else:
rospy.logwarn('Not connected to the face server, cannot follow faces.')
self.facePos = self.defaultLookatPoint
## Callback function for receiving gaze commands
def executeGazeAction(self, goal):
command = goal.action;
if (self.doNotInterrupt.count(self.currentGazeAction) == 0):
if (self.currentGazeAction != command or command == GazeGoal.LOOK_AT_POINT):
self.isActionComplete = False
if (command == GazeGoal.LOOK_FORWARD):
self.targetLookatPoint = self.defaultLookatPoint
elif (command == GazeGoal.LOOK_DOWN):
self.targetLookatPoint = self.downLookatPoint
elif (command == GazeGoal.NOD):
self.nNods = 5
self.startNod()
elif (command == GazeGoal.SHAKE):
self.nShakes = 5
self.startShake()
elif (command == GazeGoal.NOD_ONCE):
self.nNods = 5
self.startNod()
elif (command == GazeGoal.SHAKE_ONCE):
self.nShakes = 5
self.startShake()
elif (command == GazeGoal.GLANCE_RIGHT_EE):
self.startGlance(0)
elif (command == GazeGoal.GLANCE_LEFT_EE):
self.startGlance(1)
elif (command == GazeGoal.LOOK_AT_POINT):
self.targetLookatPoint = goal.point
rospy.loginfo('Setting gaze action to:' + str(command))
self.currentGazeAction = command
while (not self.isActionComplete):
time.sleep(0.1)
self.gazeActionServer.set_succeeded()
else:
self.gazeActionServer.set_aborted()
else:
self.gazeActionServer.set_aborted()
def isTheSame(self, current, target):
diff = target - current
dist = linalg.norm(diff)
return (dist<0.0001)
def filterLookatPosition(self, current, target):
speed = 0.02
diff = self.point2array(target) - self.point2array(current)
dist = linalg.norm(diff)
if (dist>speed):
step = dist/speed
return self.array2point(self.point2array(current) + diff/step)
else:
return target
def startNod(self):
self.prevTargetLookatPoint = self.targetLookatPoint
self.prevGazeAction = str(self.currentGazeAction)
self.nodCounter = 0
self.targetLookatPoint = self.nodPositions[0]
def startGlance(self, armIndex):
self.prevTargetLookatPoint = self.targetLookatPoint
self.prevGazeAction = str(self.currentGazeAction)
self.glanceCounter = 0
self.targetLookatPoint = self.getEEPos(armIndex)
def startShake(self):
self.prevTargetLookatPoint = self.targetLookatPoint
self.prevGazeAction = str(self.currentGazeAction)
self.shakeCounter = 0
self.targetLookatPoint = self.shakePositions[0]
def point2array(self, p):
return array((p.x, p.y, p.z))
def array2point(self, a):
return Point(a[0], a[1], a[2])
def getNextNodPoint(self, current, target):
if (self.isTheSame(self.point2array(current), self.point2array(target))):
self.nodCounter += 1
if (self.nodCounter == self.nNods):
self.currentGazeAction = self.prevGazeAction
return self.prevTargetLookatPoint
else:
return self.nodPositions[self.nodCounter%2]
else:
return target
def getNextGlancePoint(self, current, target):
if (self.isTheSame(self.point2array(current), self.point2array(target))):
self.glanceCounter = 1
self.currentGazeAction = self.prevGazeAction
return self.prevTargetLookatPoint
else:
return target
def getNextShakePoint(self, current, target):
if (self.isTheSame(self.point2array(current), self.point2array(target))):
self.shakeCounter += 1
if (self.shakeCounter == self.nNods):
self.currentGazeAction = self.prevGazeAction
return self.prevTargetLookatPoint
else:
return self.shakePositions[self.shakeCounter%2]
else:
return target
def update(self):
isActionPossiblyComplete = True
if (self.currentGazeAction == GazeGoal.FOLLOW_RIGHT_EE):
self.targetLookatPoint = self.getEEPos(0)
elif (self.currentGazeAction == GazeGoal.FOLLOW_LEFT_EE):
self.targetLookatPoint = self.getEEPos(1)
elif (self.currentGazeAction == GazeGoal.FOLLOW_FACE):
self.getFaceLocation()
self.targetLookatPoint = self.facePos
elif (self.currentGazeAction == GazeGoal.NOD):
self.targetLookatPoint = self.getNextNodPoint(self.currentLookatPoint, self.targetLookatPoint)
self.headGoal.min_duration = rospy.Duration(0.5)
isActionPossiblyComplete = False;
elif (self.currentGazeAction == GazeGoal.SHAKE):
self.targetLookatPoint = self.getNextShakePoint(self.currentLookatPoint, self.targetLookatPoint)
self.headGoal.min_duration = rospy.Duration(0.5)
isActionPossiblyComplete = False;
elif (self.currentGazeAction == GazeGoal.GLANCE_RIGHT_EE or self.currentGazeAction == GazeGoal.GLANCE_LEFT_EE):
self.targetLookatPoint = self.getNextGlancePoint(self.currentLookatPoint, self.targetLookatPoint)
isActionPossiblyComplete = False;
self.currentLookatPoint = self.filterLookatPosition(self.currentLookatPoint, self.targetLookatPoint)
if (self.isTheSame(self.point2array(self.headGoal.target.point), self.point2array(self.currentLookatPoint))):
if (isActionPossiblyComplete):
if (self.headActionClient.get_state() == GoalStatus.SUCCEEDED):
self.isActionComplete = True
else:
self.headGoal.target.point = self.currentLookatPoint
self.headActionClient.send_goal(self.headGoal)
time.sleep(0.02)
class ActionTasks:
def __init__(self, script_path):
rospy.init_node('task_cordinator')
rospy.on_shutdown(self.cleanup)
# Initialize a number of parameters and variables for nav locations
setup_task_environment(self)
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_en1_mbrola")
self.robot = rospy.get_param("~robot", "robbie")
#self.NavPublisher = rospy.Publisher("goto", String)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Create the sound client object
self.soundhandle = SoundClient()
# Set the wave file path if used
self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")
#define afternoon and morning
self.noon = strftime("%p:", localtime())
if self.noon == "AM:":
self.noon1 = "Goood Morning "
else:
self.noon1 ="Good After Noon "
self.local = strftime("%H:%M:", localtime())
#local time
self.local = strftime("%H:%M:", localtime())
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(1)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice)
rospy.sleep(2)
# Subscribe to the recognizer output and set the callback function
rospy.Subscriber('/nltk_interpret', String, self.commands)
self.pub = rospy.Publisher('auto_dock', String, queue_size=1)
# here we do our main text prosessing
def commands(self,text):
task = text.data
ar = task.split(':')
rospy.logwarn(ar)
driver = Driver()
if ar[0] == "stop":
self.soundhandle.say("stopping", self.voice)
self.move_base.cancel_all_goals()
self.cmd_vel_pub.publish(Twist())
if ar[0] == "go":
loc = ar[6]+ar[9]
self.soundhandle.say(loc, self.voice)
self.move_to(loc)
if ar[0] == "pick":
self.soundhandle.say(str(PickUp(task)), self.voice)
if ar[0] == "run"and ar[5] == "demo":
self.soundhandle.say("ok running show time", self.voice)
ShowTime()
if ar[0] == "get" and ar[9] == "beer":
self.soundhandle.say("ok get a beer for you", self.voice)
if ar[0] == "turn" and ar[5] == "left":
self.soundhandle.say("ok turning left", self.voice)
driver.turn(angle = -0.5 * math.pi, angularSpeed = 0.5);#90 degrees left turn
if ar[0] == "turn" and ar[5] == "right":
driver.turn(angle = 0.5 * math.pi, angularSpeed = 0.5);#90 degrees right turn
self.soundhandle.say("ok turning right", self.voice)
if ar[0] == "move" and ar[5] == "forward":
driver.driveX(distance = 1.0, speed = 0.15)
self.soundhandle.say("ok, moving forward.", self.voice)
if ar[0] == "look":
self.soundhandle.say("ok, looking "+ str(ar[5]), self.voice)
Move_Head(task)
if ar[1] == "what"and ar[5] == "this":
self.soundhandle.say(" I have no idea. My vision system is not on line.", self.voice)
if ar[1] == "what"and ar[5] == "time":
local = strftime("%H:%M:", localtime())
self.soundhandle.say(" the time is " + local , self.voice)
if ar[1] == "what"and ar[5] == "weather":
self.soundhandle.say(str(Weather()) , self.voice)
if ar[5] == "charge":
self.soundhandle.say("Starting auto dock sequence", self.voice)
self.move_base.cancel_all_goals()
self.cmd_vel_pub.publish(Twist())
self.pub.publish("dock")
if ar[11] != "":
#self.soundhandle.say(str(Hello()), self.voice)
self.soundhandle.say("hello", self.voice)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = self.room_locations[location]
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
self.soundhandle.stopAll()
self.cmd_vel_pub.publish(Twist())
rospy.loginfo("Shutting down talk node...")
import roslib
roslib.load_manifest('rospy')
roslib.load_manifest('actionlib')
roslib.load_manifest('actionlib_msgs')
roslib.load_manifest('control_msgs')
roslib.load_manifest('geometry_msgs')
import rospy
from control_msgs.msg import PointHeadAction
from control_msgs.msg import PointHeadGoal
from actionlib import SimpleActionClient
from actionlib_msgs.msg import GoalStatus
from geometry_msgs.msg import Point
if __name__ == "__main__":
rospy.init_node('head_test_node', anonymous=True)
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
# head_goal.target.point = Point(1, 0, 1) # look forward!
head_goal.target.point = Point(1, 0, 0.5) # look forward!
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(10.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
class Arm:
''' Interfacing with one arm for controlling mode and action execution'''
_is_autorelease_on = False
def __init__(self, arm_index):
self.arm_index = arm_index
self.arm_mode = ArmMode.HOLD
self.gripper_state = None
self.gripper_joint_name = self._side_prefix() + '_gripper_joint'
self.ee_name = self._side_prefix() + '_wrist_roll_link'
self.joint_names = [self._side_prefix() + '_shoulder_pan_joint',
self._side_prefix() + '_shoulder_lift_joint',
self._side_prefix() + '_upper_arm_roll_joint',
self._side_prefix() + '_elbow_flex_joint',
self._side_prefix() + '_forearm_roll_joint',
self._side_prefix() + '_wrist_flex_joint',
self._side_prefix() + '_wrist_roll_joint']
self.all_joint_names = []
self.all_joint_poses = []
self.last_ee_pose = None
self.movement_buffer_size = 40
self.last_unstable_time = rospy.Time.now()
self.arm_movement = []
rospy.loginfo('Initializing ' + self._side() + ' arm.')
switch_controller = 'pr2_controller_manager/switch_controller'
rospy.wait_for_service(switch_controller)
self.switch_service = rospy.ServiceProxy(switch_controller,
SwitchController)
rospy.loginfo('Got response form the switch controller for '
+ self._side() + ' arm.')
# Create a trajectory action client
traj_controller_name = (self._side_prefix()
+ '_arm_controller/joint_trajectory_action')
self.traj_action_client = SimpleActionClient(
traj_controller_name, JointTrajectoryAction)
self.traj_action_client.wait_for_server()
rospy.loginfo('Got response form trajectory action server for '
+ self._side() + ' arm.')
# Set up Inversse Kinematics
self.ik_srv = None
self.ik_request = None
self.ik_joints = None
self.ik_limits = None
self._setup_ik()
gripper_name = (self._side_prefix() +
'_gripper_controller/gripper_action')
self.gripper_client = SimpleActionClient(gripper_name,
Pr2GripperCommandAction)
self.gripper_client.wait_for_server()
rospy.loginfo('Got response form gripper server for '
+ self._side() + ' arm.')
filter_srv_name = '/trajectory_filter/filter_trajectory'
rospy.wait_for_service(filter_srv_name)
self.filter_service = rospy.ServiceProxy(filter_srv_name,
FilterJointTrajectory)
rospy.loginfo('Filtering service has responded for ' +
self._side() + ' arm.')
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
def _setup_ik(self):
'''Sets up services for inverse kinematics'''
ik_info_srv_name = ('pr2_' + self._side() +
'_arm_kinematics_simple/get_ik_solver_info')
ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'
rospy.wait_for_service(ik_info_srv_name)
ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
GetKinematicSolverInfo)
solver_info = ik_info_srv()
rospy.loginfo('IK info service has responded for '
+ self._side() + ' arm.')
rospy.wait_for_service(ik_srv_name)
self.ik_srv = rospy.ServiceProxy(ik_srv_name,
GetPositionIK, persistent=True)
rospy.loginfo('IK service has responded for ' + self._side() + ' arm.')
# Set up common parts of an IK request
self.ik_request = GetPositionIKRequest()
self.ik_request.timeout = rospy.Duration(4.0)
self.ik_joints = solver_info.kinematic_solver_info.joint_names
self.ik_limits = solver_info.kinematic_solver_info.limits
ik_links = solver_info.kinematic_solver_info.link_names
request = self.ik_request.ik_request
request.ik_link_name = ik_links[0]
request.pose_stamped.header.frame_id = 'base_link'
request.ik_seed_state.joint_state.name = self.ik_joints
request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints)
def _side(self):
'''Returns the word right or left depending on arm side'''
if (self.arm_index == Side.RIGHT):
return 'right'
elif (self.arm_index == Side.LEFT):
return 'left'
def _side_prefix(self):
''' Returns the letter r or l depending on arm side'''
side = self._side()
return side[0]
def get_ee_state(self, ref_frame='base_link'):
''' Returns end effector pose for the arm'''
try:
time = World.tf_listener.getLatestCommonTime(ref_frame,
self.ee_name)
(position, orientation) = World.tf_listener.lookupTransform(
ref_frame, self.ee_name, time)
ee_pose = Pose()
ee_pose.position = Point(position[0], position[1], position[2])
ee_pose.orientation = Quaternion(orientation[0], orientation[1],
orientation[2], orientation[3])
return ee_pose
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logwarn('Something wrong with transform request.')
return None
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.lock.release()
def get_joint_state(self, joint_names=None):
'''Returns position for the requested or all arm joints'''
if joint_names == None:
joint_names = self.joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received!\n")
return []
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return positions
def _solve_ik(self, ee_pose, seed=None):
'''Gets the IK solution for end effector pose'''
self.ik_request.ik_request.pose_stamped.pose = ee_pose
if seed == None:
# If no see is specified for IK search, start search at midpoint
seed = []
for i in range(0, len(self.ik_joints)):
seed.append((self.ik_limits[i].min_position +
self.ik_limits[i].max_position) / 2.0)
self.ik_request.ik_request.ik_seed_state.joint_state.position = seed
try:
rospy.loginfo('Sending IK request.')
response = self.ik_srv(self.ik_request)
if(response.error_code.val == response.error_code.SUCCESS):
return response.solution.joint_state.position
else:
return None
except rospy.ServiceException:
rospy.logerr('Exception while getting the IK solution.')
return None
def set_mode(self, mode):
'''Releases or holds the arm by turning the controller on/off'''
controller_name = self._side_prefix() + '_arm_controller'
if mode == ArmMode.RELEASE:
start_controllers = []
stop_controllers = [controller_name]
rospy.loginfo('Switching ' + str(self._side()) +
' arm to the kinesthetic mode')
elif mode == ArmMode.HOLD:
start_controllers = [controller_name]
stop_controllers = []
rospy.loginfo('Switching ' + str(self._side()) +
' to the Joint-control mode')
else:
rospy.logwarn('Unknown mode ' + str(mode) +
'. Keeping the current mode.')
return
try:
self.switch_service(start_controllers, stop_controllers, 1)
self.arm_mode = mode
except rospy.ServiceException:
rospy.logerr("Service did not process request")
def get_mode(self):
'''Returns the current arm mode (released/holding)'''
return self.arm_mode
def _send_gripper_command(self, pos=0.08, eff=30.0, wait=False):
'''Sets the position of the gripper'''
command = Pr2GripperCommandGoal()
command.command.position = pos
command.command.max_effort = eff
self.gripper_client.send_goal(command)
if wait:
self.gripper_client.wait_for_result(rospy.Duration(10.0))
def is_gripper_moving(self):
''' Whether or not the gripper is in the process of opening/closing'''
return (self.gripper_client.get_state() == GoalStatus.ACTIVE or
self.gripper_client.get_state() == GoalStatus.PENDING)
def is_gripper_at_goal(self):
''' Whether or not the gripper has reached its goal'''
return (self.gripper_client.get_state() == GoalStatus.SUCCEEDED)
def get_gripper_state(self):
'''Returns current gripper state'''
return self.gripper_state
def check_gripper_state(self, joint_name=None):
'''Checks gripper state at the hardware level'''
if (joint_name == None):
joint_name = self.gripper_joint_name
gripper_pos = self.get_joint_state([joint_name])
if gripper_pos != []:
if gripper_pos[0] > 0.078:
self.gripper_state = GripperState.OPEN
else:
self.gripper_state = GripperState.CLOSED
else:
rospy.logwarn('Could not update the gripper state.')
def open_gripper(self, pos=0.08, eff=30.0, wait=False):
'''Opens gripper'''
self._send_gripper_command(pos, eff, wait)
self.gripper_state = GripperState.OPEN
def close_gripper(self, pos=0.0, eff=30.0, wait=False):
'''Closes gripper'''
self._send_gripper_command(pos, eff, wait)
self.gripper_state = GripperState.CLOSED
def set_gripper(self, gripper_state):
'''Sets gripper to the desired state'''
if (gripper_state == GripperState.CLOSED):
self.close_gripper()
elif (gripper_state == GripperState.OPEN):
self.open_gripper()
def exectute_joint_traj(self, joint_trajectory, timing):
'''Moves the arm through the joint sequence'''
# First, do filtering on the trajectory to fix the velocities
trajectory = JointTrajectory()
# Initialize the server
# When to start the trajectory: 0.1 seconds from now
trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
trajectory.joint_names = self.joint_names
## Add all frames of the trajectory as way points
for i in range(len(timing)):
positions = joint_trajectory[i].joint_pose
velocities = [0] * len(positions)
# Add frames to the trajectory
trajectory.points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities,
time_from_start=timing[i]))
output = self.filter_service(trajectory=trajectory,
allowed_time=rospy.Duration.from_sec(20))
rospy.loginfo('Trajectory for arm ' + str(self.arm_index) +
' has been filtered.')
traj_goal = JointTrajectoryGoal()
# TO-DO: check output.error_code
traj_goal.trajectory = output.trajectory
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.joint_names = self.joint_names
# Sends the goal to the trajectory server
self.traj_action_client.send_goal(traj_goal)
def move_to_joints(self, joints, time_to_joint):
'''Moves the arm to the desired joints'''
# Setup the goal
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.joint_names = self.joint_names
velocities = [0] * len(joints)
traj_goal.trajectory.points.append(JointTrajectoryPoint(
positions=joints,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
# Client sends the goal to the Server
self.traj_action_client.send_goal(traj_goal)
def is_executing(self):
'''Whether or not there is an ongoing action execution on the arm'''
return (self.traj_action_client.get_state() == GoalStatus.ACTIVE
or self.traj_action_client.get_state() == GoalStatus.PENDING)
def is_successful(self):
'''Whetehr the execution succeeded'''
return (self.traj_action_client.get_state() == GoalStatus.SUCCEEDED)
def get_ik_for_ee(self, ee_pose, seed):
''' Finds the IK solution for given end effector pose'''
joints = self._solve_ik(ee_pose, seed)
## If our seed did not work, try once again with the default seed
if joints == None:
rospy.logwarn('Could not find IK solution with preferred seed,' +
'will try default seed.')
joints = self._solve_ik(ee_pose)
if joints == None:
rospy.logwarn('IK out of bounds, will use the seed directly.')
else:
rollover = array((array(joints) - array(seed)) / pi, int)
joints -= ((rollover + (sign(rollover) + 1) / 2) / 2) * 2 * pi
return joints
@staticmethod
def get_distance_bw_poses(pose0, pose1):
'''Returns the dissimilarity between two end-effector poses'''
w_pos = 1.0
w_rot = 0.2
pos0 = array((pose0.position.x, pose0.position.y, pose0.position.z))
pos1 = array((pose1.position.x, pose1.position.y, pose1.position.z))
rot0 = array((pose0.orientation.x, pose0.orientation.y,
pose0.orientation.z, pose0.orientation.w))
rot1 = array((pose1.orientation.x, pose1.orientation.y,
pose1.orientation.z, pose1.orientation.w))
pos_dist = w_pos * norm(pos0 - pos1)
rot_dist = w_rot * (1 - dot(rot0, rot1))
if (pos_dist > rot_dist):
dist = pos_dist
else:
dist = rot_dist
return dist
def reset_movement_history(self):
''' Clears the saved history of arm movements'''
self.last_unstable_time = rospy.Time.now()
self.arm_movement = []
def get_movement(self):
'''Returns cumulative movement in recent history'''
return sum(self.arm_movement)
def _record_arm_movement(self, reading):
'''Records the sensed arm movement'''
self.arm_movement = [reading] + self.arm_movement
if (len(self.arm_movement) > self.movement_buffer_size):
self.arm_movement = self.arm_movement[0:self.movement_buffer_size]
def _is_arm_moved_while_holding(self):
'''Checks if user is trying to move the arm while it is stiff'''
threshold = 0.02
if (self.get_mode() == ArmMode.HOLD
and (len(self.arm_movement) == self.movement_buffer_size)
and (self.get_movement() > threshold)):
return True
return False
def _is_arm_stable_while_released(self):
'''Checks if the arm has been stable while being released'''
movement_threshold = 0.02
time_threshold = rospy.Duration(5.0)
is_arm_stable = (self.get_movement() < movement_threshold)
if (not is_arm_stable or self.get_mode() == ArmMode.HOLD):
self.last_unstable_time = rospy.Time.now()
return False
else:
if (rospy.Time.now() - self.last_unstable_time) > time_threshold:
return True
else:
return False
def update(self, is_executing):
''' Periodical update for one arm'''
ee_pose = self.get_ee_state()
if (ee_pose != None and self.last_ee_pose != None):
self._record_arm_movement(Arm.get_distance_bw_poses(ee_pose,
self.last_ee_pose))
self.last_ee_pose = ee_pose
if (not is_executing and Arm._is_autorelease_on):
if (self._is_arm_moved_while_holding()):
rospy.loginfo('Automatically releasing arm.')
self.set_mode(ArmMode.RELEASE)
if (self._is_arm_stable_while_released()):
rospy.loginfo('Automatically holding arm.')
self.set_mode(ArmMode.HOLD)
class ShowTime:
def __init__(self, script_path):
rospy.init_node('show_time')
rospy.on_shutdown(self.cleanup)
# Initialize a number of parameters and variables for nav locations
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Initialize the move group for the arms
right_arm = MoveGroupCommander('right_arm')
left_arm = MoveGroupCommander('left_arm')
right_gripper = MoveGroupCommander('right_gripper')
left_gripper = MoveGroupCommander('left_gripper')
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_don_diphone")
self.robot = rospy.get_param("~robot", "robbie")
self.start =(Pose(Point(1.7, 0, 0.0), Quaternion(0.0, 0.0, 1.000, 0.018)))
self.left =(Pose(Point(1.960, -1.854, 0.0), Quaternion(0.0, 0.0, 0.916, 0.401)))
self.right =(Pose(Point(2.123, 1.592, 0.0), Quaternion(0.0, 0.0, 0.877, -0.480)))
self.auto_dock =(Pose(Point(0.5, 0, 0.0), Quaternion(0.0, 0.0, 0.002, 1.000)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(2)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
# Set the wave file path if used
self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")
#define afternoon and morning
self.noon = strftime("%p:", localtime())
if self.noon == "AM:":
self.noon1 = "Goood Morning "
else:
self.noon1 ="Good After Noon "
self.local = strftime("%H:%M:", localtime())
#local time
self.local = strftime("%H:%M:", localtime())
self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice)
right_arm.set_named_target('r_travel')
right_arm.go()
left_arm.set_named_target('l_travel')
left_arm.go()
rospy.sleep(5)
self.move_to(self.start)
self.soundhandle.say("Welcome to SHOW time. This is where I get to demonstrate my capabilities" , self.voice)
rospy.sleep(6)
self.soundhandle.say("I can move to my left", self.voice)
rospy.sleep(2)
self.move_to(self.left)
self.soundhandle.say("now back to the start position", self.voice)
rospy.sleep(2)
self.move_to(self.start)
self.soundhandle.say("I can move to my right", self.voice)
rospy.sleep(2)
self.move_to(self.right)
self.soundhandle.say("And again back to the start position", self.voice)
rospy.sleep(2)
self.move_to(self.start)
self.soundhandle.say("I once caught a fish this big", self.voice)
right_arm.set_named_target('r_fish')
right_arm.go()
left_arm.set_named_target('l_fish')
left_arm.go()
#rospy.sleep(2)
self.soundhandle.say("Thank you for your time ", self.voice)
right_arm.set_named_target('right_start')
right_arm.go()
left_arm.set_named_target('left_start')
left_arm.go()
#rospy.sleep(2)
#rospy.sleep(2)
self.move_to(self.auto_dock)
# add auto dock sequence here
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
self.soundhandle.stopAll()
rospy.loginfo("Shutting down show time node...")
class SimpleExerciser(unittest.TestCase):
def setUp(self):
self.default_wait = rospy.Duration(60.0)
self.client = SimpleActionClient('test_request_action', TestRequestAction)
self.assert_(self.client.wait_for_server(self.default_wait))
def test_just_succeed(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_just_abort(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_ABORTED,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_just_preempt(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
delay_terminate = rospy.Duration(100000),
the_result = 42)
self.client.send_goal(goal)
# Ensure that the action server got the goal, before continuing
timeout_time = rospy.Time.now() + self.default_wait
while rospy.Time.now() < timeout_time:
if (self.client.get_state() != GoalStatus.PENDING):
break
self.client.cancel_goal()
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
# Should print out errors about not setting a terminal status in the action server.
def test_drop(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_DROP,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(0, self.client.get_result().the_result)
# Should print out errors about throwing an exception
def test_exception(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_EXCEPTION,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
self.assertEqual(0, self.client.get_result().the_result)
def test_ignore_cancel_and_succeed(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
delay_terminate = rospy.Duration(2.0),
ignore_cancel = True,
the_result = 42)
self.client.send_goal(goal)
# Ensure that the action server got the goal, before continuing
timeout_time = rospy.Time.now() + self.default_wait
while rospy.Time.now() < timeout_time:
if (self.client.get_state() != GoalStatus.PENDING):
break
self.client.cancel_goal()
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
self.assertEqual(42, self.client.get_result().the_result)
def test_lose(self):
goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_LOSE,
the_result = 42)
self.client.send_goal(goal)
self.client.wait_for_result(self.default_wait)
self.assertEqual(GoalStatus.LOST, self.client.get_state())
class map_bot_base():
###########
# __init__: Class instantiator.
##########
def __init__(self):
# global
self.speed = Twist()
self.state = FSM_STATES.FSM_AUTONOMY
self.goal = MoveBaseGoal()
self.base_pose = MoveBaseGoal()
self.serv_state = String()
# move_base Action Client
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
# Subscribtions
rospy.Subscriber('/map_bot_base/goal', PoseStamped, self.goal_callback)
rospy.Subscriber('/client_node/serv_state', String, self.serv_state_callback)
# Publications
self.state_pub = rospy.Publisher('/map_bot_base/state', String, queue_size=10) # /navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
# Kill start up node to save processing power
rospy.loginfo('Killing start up node......')
subprocess.call('rosnode kill /start_up_node', shell = True)
# Set up delay to allow for hector to startup
rospy.loginfo('Starting up processes......')
for i in range(9): # 10 second delay
rospy.loginfo('Initializing behaviour in %d' % (10-i))
rospy.sleep(rospy.Duration(1))
#################
# goal_callback : Run everytime '\bin_bot_base\goal' topic receives message
# # Sends a goal to the Action Server upon callback
#################
def goal_callback(self, pose_stamped):
self.goal.target_pose = pose_stamped
if(self.goal):
self._move_base.send_goal(self.goal)
rospy.loginfo("Just passed a goal")
######################
# serv_state_callback : Run everytime the bin updates it's state.
######################
def serv_state_callback(self, serv_state):
self.serv_state = serv_state
#################
# spin_callback : Handles state transitions
#################
def spin_callback(self):
# Print state
print ("\n")
rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state))
# FSM_AUTONOMY STATE
# =============>
if self.state == FSM_STATES.FSM_AUTONOMY:
# Let hector handle navigation
# Next state transition
if (self._move_base.simple_state != 2): # if a goal is detected
rospy.loginfo('Killing hector navigation nodes.......')
subprocess.call('rosnode kill /hector_exploration_node', shell = True)
self.state = FSM_STATES.FSM_MANUAL
else:
self.state = FSM_STATES.FSM_AUTONOMY
# FSM_MANUAL STATE
# =============>
if self.state == FSM_STATES.FSM_MANUAL:
# Let move base handle navigation
# Next state transition
# if-else statement to ensure error-less view of goal status
if (self.serv_state.data == 'MAP_COMPLETE'): # if a goal is detected
self.state = FSM_STATES.FSM_MOVE_TO_BASE
else:
self.state = FSM_STATES.FSM_MANUAL
# FSM_MOVE_TO_BASE STATE
# =============>
if self.state == FSM_STATES.FSM_MOVE_TO_BASE:
# Send a move base goal to the reset position
# and go to FSM_IDLE state
self.base_pose.target_pose.header.frame_id = 'map'
self.base_pose.target_pose.header.stamp = rospy.Time.now()
self.base_pose.target_pose.pose.position.x = 0.5
self.base_pose.target_pose.pose.position.y = 0.5
self.base_pose.target_pose.pose.position.z = 0.0
self.base_pose.target_pose.pose.orientation.x = 0.0
self.base_pose.target_pose.pose.orientation.y = 0.0
self.base_pose.target_pose.pose.orientation.z = 0.0
self.base_pose.target_pose.pose.orientation.w = 0.0
self._move_base.send_goal(self.base_pose)
self.goal_state = self._move_base.get_state()
rospy.loginfo("Map complete!. Moving back to base...")
rospy.loginfo("Goal Status: %f" % self.goal_state)
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
self.state = FSM_STATES.FSM_IDLE
# FSM_IDLE STATE
# =============>
if self.state == FSM_STATES.FSM_IDLE:
# Spin
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Next state transition
# if-else statement to ensure error-less view of goal status
if (self._move_base.simple_state == 1): # if goal is detected
self.state = FSM_STATES.FSM_NAVIGATE
else:
self.state = FSM_STATES.FSM_IDLE
# Print general info
self.printINFO()
# Publish /bin_bot_base/state
self.state_pub.publish(String(FSM_STATES.__geitem__(self.state)))
##############
# printINFO(): Prints general debug info
#############
def printINFO(self):
# Print debug info about linear and angular speeds
rospy.loginfo("speedPID: %f" %(self.speed.linear.x))
rospy.loginfo("anglePID: %f" %(self.speed.angular.z))
# Print goal status info
if (self._move_base.simple_state == 2):
self.goal_state = 'No goals received'
rospy.loginfo("Goal Status: %s" %self.goal_state)
else: # if a goal is detected
self.goal_state = self._move_base.get_state()
rospy.loginfo("Goal Status: %f" %self.goal_state)
rospy.loginfo(self.serv_state.data)
#########
# spin(): Provides desired rate for speed_callback() and ensures termination
########
def spin(self):
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.spin_callback()
rate.sleep()
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 arm_bot_base():
###########
# __init__: Class instantiator.
##########
def __init__(self, KpRot, KdRot, tol_z, tol_ang, maxRotSpeed, max_z, min_z):
# global
self.speed = Twist()
self.tracked_blob = Twist()
self.old_tracked_blob = Twist()
self.state = FSM_STATES.FSM_LOCALIZE
self.obj_pose = MoveBaseGoal()
self.goal = MoveBaseGoal()
self.base_pose = MoveBaseGoal()
self.filtered_blobs_3d = Blobs3d()
self.blobs_detected = False
self.miss_counter = 0
self.correction_start = 0.0
self.serv_state = String()
self.arm_state = String()
self.blob_count = 0
self.avg_z = 0.0
self.yaw = 0.0
self.init_yaw = 0.0
self.target_yaw = 0.0
self.RotDerivator = 0.0
self.yawDerivator = 0.0
# params
self.KpRot = KpRot
self.KdRot = KdRot
self.tol_z = tol_z
self.tol_ang = tol_ang
self.maxRotSpeed = maxRotSpeed
self.max_z = max_z
self.min_z = min_z
# move_base Action Client
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
# Subscribtions
rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback)
rospy.Subscriber('/arm_bot_base/goal', PoseStamped, self.goal_callback)
rospy.Subscriber('/client_node/serv_state', String, self.serv_state_callback)
rospy.Subscriber('/uarm/state', String, self.arm_state_callback)
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.amcl_callback)
# Publications
self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
self.target_pose_pub = rospy.Publisher('/arm_bot_base/target_pose', PoseStamped)
self.state_pub = rospy.Publisher('/arm_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
# Kill start up node to save performance
rospy.loginfo('Killing start up node......')
subprocess.call('rosnode kill /start_up_node', shell = True)
# Set up delay to allow for amcl to startup
rospy.loginfo('Starting up processes......')
for i in range (9): # 10 second delay
rospy.loginfo('Initializing behaviour in %d' %(10-i))
rospy.sleep(rospy.Duration(1))
self.loc_start = rospy.Time.now()
#################
# goal_callback : Run everytime '\arm_bot_base\goal' topic receives message
# # Sends a goal to the Action Server upon callback
#################
def goal_callback(self, pose_stamped):
self.goal.target_pose = pose_stamped
if(self.goal):
self._move_base.send_goal(self.goal)
rospy.loginfo("Just passed a goal")
#################
# blob_callback : Run everytime \blobs_3d receives message. Filters blobs depending on their position to exclude blobs outside
# # the arena and a certain range. The largest blob is selected and recorded for processing.
#################
def blob_callback(self, blobs_3d):
self.old_tracked_blob = self.tracked_blob
self.filtered_blobs_3d = Blobs3d() # Initialize empty filtered blobs list
self.max_blob_area = 0 # reset maximum blob area
self.sum_x = 0.0
self.blob_count = 0
# Filter blobs
if(len(blobs_3d.blobs)):
for blob in blobs_3d.blobs:
if (blob.center.y >= 0.000 and blob.center.z <= 0.0012): # if blob is closer than 1.2m and below sensor level
self.filtered_blobs_3d.blobs.append(blob)
# if filtered list contains blobs
if(len(self.filtered_blobs_3d.blobs)):
self.blobs_detected = True # signal that a blobs have been detected
self.miss_counter = 0 # reset miss counter
# Find largest blob and track it
for blob in self.filtered_blobs_3d.blobs:
self.blob_count = self.blob_count + 1
self.sum_x = self.sum_x + blob.center.x
if(blob.area > self.max_blob_area):
self.max_blob_area = blob.area
self.tracked_blob.linear = self.getPose(blob)
self.avg_x = float(self.sum_x / self.blob_count)
self.update_avg_z()
self.tracked_blob.linear.x = self.avg_x
self.tracked_blob.linear.z = self.avg_z
self.pub_tracked_blob_tf(self.tracked_blob.linear)
# else if filtered list does not contain any blobs
else:
self.miss_counter += 1 # increment miss count
if(self.miss_counter >= 10): # Allow for 10 callbacks before signaling non-detection
self.blobs_detected = False
self.avg_z = 0
self.tracked_blob = Twist()
######################
# arm_state_callback : Run everytime the arm updates it's state.
######################
def arm_state_callback(self, arm_state):
self.arm_state = arm_state
######################
# serv_state_callback : Run everytime the server updates it's state.
######################
def serv_state_callback(self, serv_state):
self.serv_state = serv_state
if self.serv_state.data == "RESET":
self.state = FSM_STATES.FSM_RESET
self.reset_time = rospy.Time.now() # Used during initial wait
#################
# amcl_callback : Debug callback to compute yaw
#################
def amcl_callback(self, robot_pose):
self.robot_pose = robot_pose
(roll,pitch,self.yaw) = euler_from_quaternion([self.robot_pose.pose.pose.orientation.x, \
self.robot_pose.pose.pose.orientation.y, self.robot_pose.pose.pose.orientation.z, self.robot_pose.pose.pose.orientation.w])
#################
# speed_callback : Pubishes velocity and handles state transitions
#################
def speed_callback(self):
self.zStable = False # flag to signify desired z satisfied
self.angStable = False # flag to signify desired angle satisfied
self.sleepDuration = rospy.Duration(3) # duration of forward movement
# Print state
print ("\n")
rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state))
# FSM_LOCALIZE STATE
# =============>
if self.state == FSM_STATES.FSM_LOCALIZE:
# Spin
self.speed.linear.x = 0.0
self.speed.angular.z = 0.5
# Next state transition
# if-else statement to ensure error-less view of goal status
if(rospy.Time.now() < self.loc_start + rospy.Duration(20)):
self.state = FSM_STATES.FSM_LOCALIZE
else:
self.state = FSM_STATES.FSM_IDLE
# FSM_IDLE STATE
# =============>
if self.state == FSM_STATES.FSM_IDLE:
# Stay still
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Next state transition
# if-else statement to ensure error-less view of goal status
if (self._move_base.simple_state == 1): # if goal is detected
self.state = FSM_STATES.FSM_NAVIGATE
else:
self.state = FSM_STATES.FSM_IDLE
# FSM_RESET STATE
# =============>
if self.state == FSM_STATES.FSM_RESET:
self.reset_goal = MoveBaseGoal()
self.reset_goal.target_pose.header.frame_id = 'map'
self.reset_goal.target_pose.header.stamp = rospy.Time.now()
self.reset_goal.target_pose.pose.position.x = 0.0
self.reset_goal.target_pose.pose.position.y = -2.0
self.reset_goal.target_pose.pose.position.z = 0.0
self.reset_goal.target_pose.pose.orientation.x = 0.0
self.reset_goal.target_pose.pose.orientation.y = 0.0
self.reset_goal.target_pose.pose.orientation.z = 0.5
self.reset_goal.target_pose.pose.orientation.w = 1.00
self._move_base.send_goal(self.reset_goal)
self.goal_state = self._move_base.get_state()
rospy.loginfo("Reset signal received by server. Moving back to base...")
rospy.loginfo("Goal Status: %f" %self.goal_state)
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
rospy.wait_for_service('/move_base/clear_costmaps')
self.clear_costmaps = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
try:
rospy.wait_for_service('/move_base/clear_costmaps')
self.clear_costmaps()
except rospy.ServiceException as exc:
print("Service did not process request: " + str(exc))
self.state = FSM_STATES.FSM_IDLE
# FSM_NAVIGATE STATE
# =============>
elif self.state == FSM_STATES.FSM_NAVIGATE:
# No action required, simply allow path planner to navigate
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Print debug info
if (self._move_base.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('Search for object failed!')
# Next state transitions
if(self.blobs_detected): # if a blob is seen
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
rospy.loginfo("Object detected! Aborting goal and approaching object...")
self.state = FSM_STATES.FSM_OPTIMIZE
else:
if(self._move_base.get_state() != 3.0):
rospy.loginfo("Navigating to object at pose (%f, %f, %f)..." %(self.goal.target_pose.pose.position.x, self.goal.target_pose.pose.position.y, self.goal.target_pose.pose.position.z))
else:
rospy.loginfo("Waiting for goal...")
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
self.state = FSM_STATES.FSM_NAVIGATE
# FSM_SEARCH STATE
# ===============>
elif self.state == FSM_STATES.FSM_SEARCH:
# Spin to search for object
self.speed.linear.x = 0.0
self.speed.angular.z = 1.0
# Next state transitions
if (self.blobs_detected):
self.state = FSM_STATES.FSM_OPTIMIZE
elif (self._move_base.simple_state == 1): # if goal is detected
self.state = FSM_STATES.FSM_NAVIGATE
else:
self.state = FSM_STATES.FSM_SEARCH
# FSM_OPTIMIZE STATE (PID/PD controller)
# =================>
elif self.state == FSM_STATES.FSM_OPTIMIZE :
self._move_base.cancel_all_goals()
rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z))
self.RotDerivator = 0.0
# if statement to check object is with range (TO BE DELETED/AMENDED!)
if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z):
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
rospy.loginfo("Distance = %f" %(self.tracked_blob.linear.z))
rospy.loginfo("Detected object is either too close or too far away.")
else:
# Set linear speed to 0
self.speed.linear.x = 0.0
# Compute PD angular error
self.angError = -0.1*math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y);
self.pRotError = self.KpRot*self.angError
self.dRotError = self.KdRot*(self.angError-self.RotDerivator)
self.RotDerivator = self.angError
self.anglePID = self.pRotError + self.dRotError;
# Cap maximum rotation speed
if(math.fabs(self.anglePID)>self.maxRotSpeed):
self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID
# Set tolerance in angle to signify stable orientation
if(math.fabs(self.angError) < self.tol_ang):
self.speed.angular.z = 0.0
self.RotDerivator = 0.0
self.angStable = True
else:
self.speed.angular.z = self.anglePID
self.angStable = False
# Debug messages
rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z))
rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError))
# Next state transitions
if (self.angStable): # if stable position and orientation have been achieved
self.goal = MoveBaseGoal() # reset goal
if (self.tracked_blob.linear.z > 0.001): # if object is cloaser than 1 m
self.state = FSM_STATES.FSM_APPROACH_OBJ
else:
self.correction_start = rospy.Time.now()
self.state = FSM_STATES.FSM_STABLE # move to stable state
elif(self.blobs_detected): # else if (while) blobs are being detected
self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state
else:
#self._move_base.send_goal(self.goal)
#rospy.sleep(self.sleepDuration)
#self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state
self.state = FSM_STATES.FSM_SEARCH
# FSM_APPROACH_OBJ STATE
# =============>
elif self.state == FSM_STATES.FSM_APPROACH_OBJ:
# Set constants and time in goal pose
self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame'
self.obj_pose.target_pose.header.stamp = rospy.Time.now()
self.obj_pose.target_pose.pose.orientation.w = 1.0
# if-else statements to perform corrections in target pose
# if distance is less than 2m and more than 1.50m
#if (self.tracked_blob.linear.z > 0.0015):
# self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.001) # set goal to (distance - 1m)
# self.target_pose_pub.publish(self.obj_pose.target_pose)
# self._move_base.send_goal(self.obj_pose) # send goal
# rospy.loginfo('Performing approach to 1m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
# rospy.loginfo('Waiting for result...')
# self._move_base.wait_for_result()
# if self._move_base.get_state() == GoalStatus.SUCCEEDED:
# rospy.loginfo('Goal Reached!')
# else:
# rospy.loginfo('Goal aborted!')
# else if distance is less than 1.50m
#else:
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.7 # set goal to final target
self.target_pose_pub.publish(self.obj_pose.target_pose)
self._move_base.send_goal(self.obj_pose) # send goal
rospy.loginfo('Performing final approach. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
rospy.loginfo('Waiting for result...')
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
# Next state transitions
if (self.blobs_detected):
self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle
else:
self.state = FSM_STATES.FSM_SEARCH
# FSM_STABLE STATE
# ===============>
elif (self.state == FSM_STATES.FSM_STABLE):
# Allow 2 seconds for blob distance to saturate and then compute forward movement duration
if (rospy.Time.now() < self.correction_start + rospy.Duration(2)):
rospy.loginfo('Correction of detection errors...')
self.state = FSM_STATES.FSM_STABLE
else:
self.moveDuration = rospy.Duration((self.tracked_blob.linear.z - 0.00015)/0.0001)
rospy.loginfo((self.tracked_blob.linear.z - 0.0002)/0.0001)
self.moveStartTime = rospy.Time.now()
self.state = FSM_STATES.FSM_FINAL_APPROACH
# FSM_FINAL_APPROACH
# ==================>
elif (self.state == FSM_STATES.FSM_FINAL_APPROACH):
# Move forward for desired duration to approach target object
if (rospy.Time.now() < self.moveStartTime + self.moveDuration):
self.speed.linear.x = 0.1
self.speed.angular.z = 0.0
self.state = FSM_STATES.FSM_FINAL_APPROACH
# Once duration has elapsed move to spin state
else:
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
self.state = FSM_STATES.FSM_WAIT_FOR_ACTION
# FSM_SPIN STATE
# =============>
elif (self.state == FSM_STATES.FSM_SPIN):
# Spin
self.speed.linear.x = 0.0
self.speed.angular.z = -1.0
# Next state transitions
# -Until arm sees object-! (Current content is temporary)
if (self.arm_state.data == 'ALIGN_CAMERA' or self.arm_state.data =='ALIGN_BIN'): # if arm signifies that object has been dropped
self.state = FSM_STATES.FSM_WAIT_FOR_ACTION # change to WAIT_FOR_ACTION state
elif (self._move_base.simple_state != 2): # if a goal is detected
self.state = FSM_STATES.FSM_MOVE_TO_BASE
else:
self.state = FSM_STATES.FSM_SPIN
# FSM_WAIT_FOR_ACTION STATE
# =============>
elif (self.state == FSM_STATES.FSM_WAIT_FOR_ACTION):
# Stop motors
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Print arm state for debug
rospy.loginfo(self.arm_state.data)
# Next state transitions
if ((self.arm_state.data == 'LOCATE_BIN') or (self.arm_state.data == 'SEARCH_OBJ')):
self.state = FSM_STATES.FSM_SPIN
elif (self.arm_state.data == 'IDLE'): # if arm signifies that object has been dropped
self.state = FSM_STATES.FSM_IDLE # change to MOVE_TO_BASE state
else: # if object not dropped
self.state = FSM_STATES.FSM_WAIT_FOR_ACTION # loop back
# FSM_MOVE_TO_BASE STATE
# =============>
elif (self.state == FSM_STATES.FSM_MOVE_TO_BASE):
self.goal_state = self._move_base.get_state()
rospy.loginfo("Moving back to base")
rospy.loginfo("Goal Status: %f" %self.goal_state)
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
self.state = FSM_STATES.FSM_NAVIGATE
rospy.wait_for_service('/move_base/clear_costmaps')
self.clear_costmaps = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
try:
rospy.wait_for_service('/move_base/clear_costmaps')
self.clear_costmaps()
except rospy.ServiceException as exc:
print("Service did not process request: " + str(exc))
# Print general info
self.printINFO()
# Publish /arm_bot_base/state
self.state_pub.publish(String(FSM_STATES.__geitem__(self.state)))
# Only publish velocity when no goal is being pursued or expected
if (self.state != FSM_STATES.FSM_NAVIGATE):
self.speed_pub.publish(self.speed)
############
# getPose(): Returns pose of received blob relative to the camera.
############
def getPose(self, blob):
self.pose = Vector3()
self.pose.x = blob.center.x
self.pose.y = blob.center.y
self.pose.z = blob.center.z
return self.pose
#################
# update_avg_z(): Updates the avg_value of distance z from blob.
# # Higher weight is given to blobs received with lower distance than the average,
# # and vice versa, such that the closest distance is ultimately tracked.
#################
def update_avg_z(self):
if (self.old_tracked_blob.linear.z == 0):
self.avg_z = self.tracked_blob.z
else:
if(self.tracked_blob.linear.z < self.avg_z): # Give heigher weight to closer blobs
self.avg_z = (self.avg_z + 10*self.tracked_blob.linear.z)/(11)
else:
self.avg_z = (10*self.avg_z + self.tracked_blob.linear.z)/(11)
########################
# pub_tracked_blob_tf(): Publishes a transform frame for the tracked blob.
#######################
def pub_tracked_blob_tf(self, pose):
br = tf.TransformBroadcaster()
br.sendTransform((1000*pose.z, -1000*pose.x, -1000*pose.y), tf.transformations.quaternion_from_euler(0, 0, 1), rospy.Time.now(), 'tracked_blob', 'camera_depth_frame')
##############
# printINFO(): Prints general debug info
#############
def printINFO(self):
# Print debug info about linear and angular speeds
rospy.loginfo("speedPID: %f" %(self.speed.linear.x))
rospy.loginfo("anglePID: %f" %(self.speed.angular.z))
# Print goal status info
if (self._move_base.simple_state == 2):
self.goal_state = 'No goals received'
rospy.loginfo("Goal Status: %s" %self.goal_state)
else: # if a goal is detected
self.goal_state = self._move_base.get_state()
rospy.loginfo("Goal Status: %f" %self.goal_state)
# Print blob info
if(self.blobs_detected):
rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z))
rospy.loginfo("miss_counter = %f" %(self.miss_counter))
else:
rospy.loginfo("No blobs detected")
rospy.loginfo("miss_counter = %f" %(self.miss_counter))
# Print yaw (**DEBUG**)
rospy.loginfo(self.yaw)
rospy.loginfo(self.arm_state.data)
#########
# spin(): Provides desired rate for speed_callback() and ensures termination
########
def spin(self):
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.speed_callback()
rate.sleep()
class Gripper():
""" Wrapper for all the action client stuff """
def __init__(self):
""" Instantiate action clients and wait for communication with servers """
self.client_gripper = SimpleActionClient("r_gripper_sensor_controller/gripper_action", Pr2GripperCommandAction)
self.client_contact = SimpleActionClient("r_gripper_sensor_controller/find_contact", PR2GripperFindContactAction)
self.client_force = SimpleActionClient("r_gripper_sensor_controller/force_servo", PR2GripperForceServoAction)
self.client_gripper.wait_for_server(rospy.Duration(10.0))
self.client_contact.wait_for_server(rospy.Duration(10.0))
self.client_force.wait_for_server(rospy.Duration(10.0))
rospy.loginfo('All servers ready!')
rospy.Subscriber('r_gripper_pressure_balance', String, self.store_balance)
def store_balance(self, balance):
""" Stores gripper balance. """
self.balance = balance.data
def open(self):
""" Open the gripper """
goal_open = Pr2GripperCommandGoal()
goal_open.command.position = 0.09
goal_open.command.max_effort = -1.0
rospy.loginfo("Sending goal: OPEN")
self.client_gripper.send_goal_and_wait(goal_open)
if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. The gripper opened!')
else:
rospy.loginfo('FAILURE. The gripper failed to open.')
def open_manual(self, position):
""" Open the gripper """
goal_open = Pr2GripperCommandGoal()
goal_open.command.position = position
goal_open.command.max_effort = -1.0
rospy.loginfo("Sending goal: OPEN")
self.client_gripper.send_goal_and_wait(goal_open)
if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. The gripper opened!')
else:
rospy.loginfo('FAILURE. The gripper failed to open.')
def close(self):
""" Close the gripper """
goal_close = Pr2GripperCommandGoal()
goal_close.command.position = 0.00
goal_close.command.max_effort = 20.0
rospy.loginfo("Sending goal: CLOSE")
self.client_gripper.send_goal_and_wait(goal_close)
if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. The gripper closed!')
else:
rospy.loginfo('FAILURE. The gripper failed to close.')
def hold(self, holdForce):
""" Hold something with a specified grip force """
goal_squeeze = PR2GripperForceServoGoal()
goal_squeeze.command.fingertip_force = holdForce
rospy.loginfo("Sending goal: HOLD with {0}N of force.".format(holdForce))
self.client_force.send_goal_and_wait( goal_squeeze )
if (self.client_force.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. Stable force achieved!')
else:
rospy.loginfo('FAILURE. Stable force was NOT achieved.')
def findTwoContacts(self):
""" Find two contacts and go into force control mode """
goal_contact = PR2GripperFindContactGoal()
goal_contact.command.contact_conditions = goal_contact.command.BOTH # close until both fingers contact
goal_contact.command.zero_fingertip_sensors = True # zero fingertip sensors before moving
rospy.loginfo("Sending goal: FIND TWO CONTACTS")
self.client_contact.send_goal_and_wait(goal_contact)
if (self.client_contact.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo('SUCCESS. Contact found at left: {0} and right: {1}'.format(self.client_contact.get_result().data.left_fingertip_pad_contact,
self.client_contact.get_result().data.right_fingertip_pad_contact))
rospy.loginfo('Contact force for left: {0} and right: {1}'.format(self.client_contact.get_result().data.left_fingertip_pad_force,
self.client_contact.get_result().data.right_fingertip_pad_force))
else:
rospy.loginfo('FAILURE. No contact found or force not able to be maintained')
class arm_bot_base():
def __init__(self, desired_z, KpLin, KdLin, KpRot, KdRot, tol_z, tol_ang, maxLinSpeed, maxRotSpeed, max_z, min_z):
#global
self.speed = Twist()
self.tracked_blob = Twist()
self.state = FSM_STATES.FSM_LOCALIZE
self.obj_pose = MoveBaseGoal()
self.goal = MoveBaseGoal()
self.blobs_3d = Blobs3d()
#params
self.desired_z = desired_z #0.001 #0.8
self.KpLin = KpLin #1000.0
self.KdLin = KdLin
self.KpRot = KpRot #0.1
self.KdRot = KdRot
self.tol_z = tol_z #0.00005 #0.05
self.tol_ang= tol_ang #0.5
self.maxLinSpeed = maxLinSpeed #0.2
self.maxRotSpeed = maxRotSpeed
self.max_z = max_z #3.0
self.min_z= min_z #0.0005 #0.2
# move_base Action Client
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
#subscribtions
rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback)
rospy.Subscriber('/arm_bot_base/goal', PoseStamped, self.goal_callback)
# publications
self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
self.state_pub = rospy.Publisher('/arm_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
#################
# goal_callback : Run everytime '\arm_bot_base\goal' topic receives message.
# # Send a goal to the Action Server upon callback
#################
def goal_callback(self, pose_stamped):
self.goal.target_pose = pose_stamped
if(self.goal):
self._move_base.send_goal(self.goal)
rospy.loginfo("Just passed a goal")
#################
# blob_callback : Run everytime \blobs_3d receives message.
# # Used to set rate of node and calls speed_callback() on every run
#################
def blob_callback(self, blobs_3d):
self.blobs_3d = blobs_3d
self.blob_area = 0
if(len(blobs_3d.blobs)):
for blob in blobs_3d.blobs:
if (blob.area>self.blob_area):
self.blob_area = blob.area
self.tracked_blob.linear = self.getPose(blob)
rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z))
#self.state = FSM_STATES.FSM_OPTIMIZE
else:
rospy.loginfo("No blobs detected")
#self.state = FSM_STATES.FSM_SEARCH
#################
# speed_callback : Pubishes velocity and handles state transitions
#################
def speed_callback(self):
self.zStable = False # flag to signify desired z satisfied
self.angStable = False # flag to signify desired angle satisfied
self.moveDuration = rospy.Duration(3) # duration of forward movement
print ("\n")
rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state))
# FSM_LOCALIZE STATE
# =============>
if self.state == FSM_STATES.FSM_LOCALIZE:
# Spin
self.speed.linear.x = 0.0
self.speed.angular.z = 0.5
# Next state transition
# if-else statement to ensure error-less view of goal status
if (self._move_base.simple_state == 2):
self.goal_state = 'No goals received'
rospy.loginfo("Goal Status: %s" %self.goal_state)
self.state = FSM_STATES.FSM_LOCALIZE
elif(len(self.blobs_3d.blobs)): # if a blob is seen
rospy.loginfo("Object detected! Approaching object...")
self.state = FSM_STATES.FSM_OPTIMIZE
else: # if a goal is detected
self.goal_state = self._move_base.get_state()
rospy.loginfo("Goal Status: %f" %self.goal_state)
self.state = FSM_STATES.FSM_NAVIGATE
# FSM_NAVIGATE STATE
# =============>
elif self.state == FSM_STATES.FSM_NAVIGATE:
# No action required, simply allow path planner to navigate
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Next state transitions
if(len(self.blobs_3d.blobs) ): # if a blob is seen
self._move_base.cancel_all_goals() # cancell all nav goals
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
rospy.loginfo("Object detected! Aborting goal and approaching object...")
self.state = FSM_STATES.FSM_OPTIMIZE
else:
self.state = FSM_STATES.FSM_NAVIGATE
# FSM_SEARCH STATE
# ===============>
elif self.state == FSM_STATES.FSM_SEARCH:
# Spin to search for object
self.speed.linear.x = 0.0
self.speed.angular.z = 0.2
# Next state transitions
if (len(self.blobs_3d.blobs)):
self.state = FSM_STATES.FSM_OPTIMIZE
else:
self.state = FSM_STATES.FSM_SEARCH
# FSM_OPTIMIZE STATE (PID/PD controller)
# =================>
elif self.state == FSM_STATES.FSM_OPTIMIZE :
rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z))
#self.LinDerivator = 0.0
self.RotDerivator = 0.0
# if statement to check object is with range (TO BE DELETED/AMENDED!)
if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z):
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
rospy.loginfo("Detected object is either too close or too far away.")
else:
# Compute distance PD error
#self.zError = self.tracked_blob.linear.z - self.desired_z
#self.pLinError = self.zError*self.KpLin
#self.dLinError = self.KdLin*(self.zError-self.LinDerivator)
#self.LinDerivator = self.zError
#self.speedPID = self.pLinError + self.dLinError
# Cap maximum linear speed
#if(math.fabs(self.speedPID)>self.maxLinSpeed):
# self.speedPID=self.maxLinSpeed*math.fabs(self.speedPID)/self.speedPID;
# Set tolerance in z to signify stable position
#if(math.fabs(self.zError)<self.tol_z):
# self.speed.linear.x = 0.0
# LinDerivator = 0.0
# self.zStable = True
#else:
# self.speed.linear.x = self.speedPID
# self.zStable = False
# Set linear speed to 0
self.speed.linear.x = 0.0
# Compute PD angular error
self.angError = -math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y);
self.pRotError = self.KpRot*self.angError
self.dRotError = self.KdRot*(self.angError-self.RotDerivator)
self.RotDerivator = self.angError
self.anglePID = self.pRotError + self.dRotError;
# Cap maximum rotation speed
if(math.fabs(self.anglePID)>self.maxRotSpeed):
self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID;
# Set tolerance in angle to signify stable orientation
if(math.fabs(self.angError) < self.tol_ang):
self.speed.angular.z = 0.0;
RotDerivator = 0.0
self.angStable = True
else:
self.speed.angular.z = self.anglePID;
self.angStable = False
# Debug messages
rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z))
rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError))
# Next state transitions
if (self.angStable): # if stable position and orientation have been achieved
self.state = FSM_STATES.FSM_APPROACH_OBJ # move to stable state
self.moveStartTime = rospy.Time.now()
elif(len(self.blobs_3d.blobs)): # else if (while) blobs are being detected
self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state
else:
self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state
# FSM_APPROACH_OBJ STATE
# =============>
elif self.state == FSM_STATES.FSM_APPROACH_OBJ:
self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame'
self.obj_pose.target_pose.header.stamp = rospy.Time.now()
self.obj_pose.target_pose.pose.orientation.w = 1.0
# if-else statements to perform corrections in target pose
# if distance is higher than 2.20m (4.5 m gazebo)
if ( self.tracked_blob.linear.z > 0.0045):
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.004) # set goal to (distance - 2m)
self._move_base.send_goal(self.obj_pose)
rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
self._move_base.wait_for_result()
rospy.loginfo(self._move_base.get_result())
self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle
# else if distance is less than 2m and more than 1.20m (2.5 m gazebo)
elif (self.tracked_blob.linear.z > 0.0025):
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.002) # set goal to (distance - 1m)
self._move_base.send_goal(self.obj_pose)
rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
self._move_base.wait_for_result()
rospy.loginfo(self._move_base.get_result())
self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle
# else if distance is less than 1m
else:
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # set goal to final target
self._move_base.send_goal(self.obj_pose)
rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
self._move_base.wait_for_result()
rospy.loginfo(self._move_base.get_result())
self.state = FSM_STATES.FSM_SPIN # move to FSM_SPIN
# if ( self.tracked_blob.linear.z > 0.004):
# while(self.tracked_blob.linear.z > 0.003):
# pass
# self._move_base.cancel_all_goals() # cancell all nav goals
# self.state = FSM_STATES.FSM_OPTIMIZE
# elif ( self.tracked_blob.linear.z > 0.001):
# while(self.tracked_blob.linear.z > 0.001):
# pass
# self._move_base.cancel_all_goals() # cancell all nav goals
# self.state = FSM_STATES.FSM_OPTIMIZE
# else:
# self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame'
# self.obj_pose.target_pose.header.stamp = rospy.Time.now()
# self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4
# self.obj_pose.target_pose.pose.orientation.w = 1.0
# self._move_base.send_goal(self.obj_pose)
# rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
# self._move_base.wait_for_result()
# rospy.loginfo(self._move_base.get_result())
# self.state= FSM_STATES.FSM_SPIN
# FSM_STABLE STATE
# ===============>
elif (self.state == FSM_STATES.FSM_STABLE):
# Move forward for desired duration to approach target object
if (rospy.Time.now() < self.moveStartTime + self.moveDuration):
self.speed.linear.x = 0.2
self.state = FSM_STATES.FSM_STABLE
# Once duration has elapsed move to spin state
else:
self.speed.linear.x = 0.0
self.state = FSM_STATES.FSM_SPIN
# FSM_SPIN STATE
# =============>
elif (self.state == FSM_STATES.FSM_SPIN):
# Spin
self.speed.linear.x = 0.0
self.speed.angular.z = 0.5
# Next state transitions
# -Until arm sees object-!
self.state_pub.publish(String(FSM_STATES.__geitem__(self.state)))
# Print debug info about linear and angular speeds
rospy.loginfo("speedPID: %f" %(self.speed.linear.x))
rospy.loginfo("anglePID: %f" %(self.speed.angular.z))
#Only publish velocity when no goal is being pursued or expected
if (self.state != FSM_STATES.FSM_NAVIGATE):
self.speed_pub.publish(self.speed)
############
# getPose(): Returns pose of received blob relative to the camera.
############
def getPose(self, blob):
self.pose = Vector3()
self.pose.x = blob.center.x
self.pose.y = blob.center.y
self.pose.z = blob.center.z
return self.pose
def spin(self):
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.speed_callback()
rate.sleep()
class ConstructStaticCollisionMapServer:
def __init__(self,
node_name = 'construct_static_collision_map_server',
action_name = 'construct_static_collision_map_action',
head_topic = '/head_traj_controller/point_head_action',
static_collision_map_topic = '/make_static_collision_map'
):
rospy.init_node(node_name)
rospy.loginfo ( 'waiting for SimpleActionClient: %s'%( head_topic ) )
self.head_client = SimpleActionClient(head_topic,PointHeadAction )
self.head_client.wait_for_server( )
self.take_static_collision_map_client = SimpleActionClient(static_collision_map_topic, MakeStaticCollisionMapAction)
self._action_server = SimpleActionServer(action_name, ConstructStaticCollisionMapAction, execute_cb=self.execute_cb)
rospy.loginfo('ready')
def execute_cb(self, goal):
self.scan_table()
goal = MakeStaticCollisionMapGoal()
self.take_static_collision_map_client.send_goal(goal)
self._action_server.set_succeeded()
def move_head(self, x, y, z,
min_dur = 0.0,
max_velocity = 1.0,
frame_id = 'base_link',
timeout = 5.0):
point = PointStamped()
point.header.frame_id = frame_id
point.header.stamp = rospy.Time.now()
point.point.x, point.point.y, point.point.z = x, y, z
goal = PointHeadGoal()
goal.pointing_frame = 'head_plate_frame'
goal.max_velocity = max_velocity
goal.min_duration = rospy.Duration.from_sec( min_dur )
goal.target = point
self.head_client.send_goal( goal )
self.head_client.wait_for_result( timeout =
rospy.Duration.from_sec( timeout ) )
if not self.head_client.get_state() == GoalStatus.SUCCEEDED:
rospy.logerr( 'can not move head to:\n %s'%( goal ) )
return False
return True
def scan_table(self,
x = 0.5,
y_min = -1.0, y_max = 1.0,
z = 0.7,
steps = 10,
min_dur = 1.0,
max_wait_time = 10.0
):
rospy.loginfo( 'scanning table with x = %5.3f, y_min = %5.3f, y_max = %5.3f, z = %5.3f, steps = %d'%( x, y_min, y_max, z, steps ) )
Y = np.linspace( y_min, y_max, steps )
for y in Y:
if not self.move_head( x, y, z, min_dur ):
return
rospy.loginfo( 'done scanning table!' )
class bin_bot_base():
def __init__(self, desired_z, KpLin, KdLin, KpRot, KdRot, tol_z, tol_ang, maxLinSpeed, maxRotSpeed, max_z, min_z):
#global
self.speed = Twist() # variable to store velocity commands
self.tracked_blob = Twist() # variable to store pose of tracked blob
self.state = FSM_STATES.FSM_LOCALIZE # set initial state
self.obj_pose = MoveBaseGoal() # variable
self.goal = MoveBaseGoal()
self.blobs_3d = Blobs3d()
self.blobs_detected = False
self.miss_counter = 0
self.zStable = False # flag to signify desired z satisfied
self.angStable = False # flag to signify desired angle satisfied
self.arm_state = String
#params
self.desired_z = desired_z #0.001 #0.8
self.KpLin = KpLin #1000.0
self.KdLin = KdLin
self.KpRot = KpRot #0.1
self.KdRot = KdRot
self.tol_z = tol_z #0.00005 #0.05
self.tol_ang= tol_ang #0.5
self.maxLinSpeed = maxLinSpeed #0.2
self.maxRotSpeed = maxRotSpeed
self.max_z = max_z #3.0
self.min_z= min_z #0.0005 #0.2
# move_base Action Client
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
self._move_base.wait_for_server()
#subscribtions
rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback)
rospy.Subscriber('/bin_bot_base/goal', PoseStamped, self.goal_callback)
rospy.Subscriber('/arm_node/state', String, self.arm_state_callback)
# publications
self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
self.state_pub = rospy.Publisher('/bin_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity
#################
# goal_callback : Run everytime '\arm_bot_base\goal' topic receives message.
# # Send a goal to the Action Server upon callback
#################
def goal_callback(self, pose_stamped):
self.goal.target_pose = pose_stamped
if(self.goal):
self._move_base.send_goal(self.goal)
rospy.loginfo("Just passed a goal")
#################
# blob_callback : Run everytime \blobs_3d receives message.
#################
def blob_callback(self, blobs_3d):
self.blobs_3d = blobs_3d
self.blob_area = 0
if(len(self.blobs_3d.blobs)):
self.blobs_detected = True
self.miss_counter = 0
for blob in blobs_3d.blobs:
if (blob.area>self.blob_area):
self.blob_area = blob.area
self.tracked_blob.linear = self.getPose(blob)
else:
if(self.miss_counter>=5): # Allow for 5 callbacks before signaling non-detection
self.blobs_detected = False
self.miss_counter += 1
######################
# arm_state_callback : Run everytime the arm updates it's state.
######################
def arm_state_callback(self, arm_state):
self.arm_state = arm_state
#################
# speed_callback : Pubishes velocity and handles state transitions
#################
def speed_callback(self):
self.moveDuration = rospy.Duration(3) # duration of forward movement
# Print state
print ("\n")
rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state))
# FSM_LOCALIZE STATE
# =============>
if self.state == FSM_STATES.FSM_LOCALIZE:
# Spin
self.speed.linear.x = 0.0
self.speed.angular.z = 0.5
# Next state transition
# if-else statement to ensure error-less view of goal status
if(self.blobs_detected): # if a blob is seen
rospy.loginfo("Arm Bot detected! Approaching robot...")
self.state = FSM_STATES.FSM_OPTIMIZE
elif (self._move_base.simple_state == 2):
self.goal_state = 'No goals received'
rospy.loginfo("Goal Status: %s" %self.goal_state)
self.state = FSM_STATES.FSM_LOCALIZE
else: # if a goal is detected
self.goal_state = self._move_base.get_state()
rospy.loginfo("Goal Status: %f" %self.goal_state)
self.state = FSM_STATES.FSM_NAVIGATE
# FSM_NAVIGATE STATE
# =============>
elif self.state == FSM_STATES.FSM_NAVIGATE:
# No action required, simply allow path planner to navigate
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Next state transitions
if(self.blobs_detected): # and (self.tracked_blob.linear.z < 3.0): # if a blob is seen (and closer than 3 meters)
self._move_base.cancel_all_goals() # cancell all nav goals
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
rospy.loginfo("Arm Bot detected! Aborting goal and approaching robot...")
self.state = FSM_STATES.FSM_OPTIMIZE
else:
if(self._move_base.get_state() != 3.0):
rospy.loginfo("Navigating to Arm Bot at pose (%f, %f, %f)..." %(self.goal.target_pose.pose.position.x, self.goal.target_pose.pose.position.y, self.goal.target_pose.pose.position.z))
else:
rospy.loginfo("Waiting for goal...")
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
self.state = FSM_STATES.FSM_NAVIGATE
# FSM_OPTIMIZE STATE (PID/PD controller)
# =================>
elif self.state == FSM_STATES.FSM_OPTIMIZE :
rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z))
#self.LinDerivator = 0.0
self.RotDerivator = 0.0
# if statement to check Arm Bot is with range (TO BE DELETED/AMENDED!)
if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z):
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
rospy.loginfo("Detected object is either too close or too far away.")
else:
# Set linear speed to 0
self.speed.linear.x = 0.0
# Compute PD angular error
self.angError = -math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y);
self.pRotError = self.KpRot*self.angError
self.dRotError = self.KdRot*(self.angError-self.RotDerivator)
self.RotDerivator = self.angError
self.anglePID = self.pRotError + self.dRotError;
# Cap maximum rotation speed
if(math.fabs(self.anglePID)>self.maxRotSpeed):
self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID;
# Set tolerance in angle to signify stable orientation
if(math.fabs(self.angError) < self.tol_ang):
self.speed.angular.z = 0.0;
RotDerivator = 0.0
self.angStable = True
else:
self.speed.angular.z = self.anglePID;
self.angStable = False
# Debug messages
rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z))
rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError))
# Next state transitions
if (self.angStable): # if stable position and orientation have been achieved
self.state = FSM_STATES.FSM_APPROACH_OBJ # move to stable state
self.moveStartTime = rospy.Time.now()
elif(self.blobs_detected): # else if (while) blobs are being detected
self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state
else:
self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state
# FSM_APPROACH_OBJ STATE
# =============>
elif self.state == FSM_STATES.FSM_APPROACH_OBJ:
self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame'
self.obj_pose.target_pose.header.stamp = rospy.Time.now()
self.obj_pose.target_pose.pose.orientation.w = 1.0
# if-else statements to perform corrections in target pose
# if distance is higher than 2.20m
if ( self.tracked_blob.linear.z > 0.0022):
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.002) # set goal to (distance - 2m)
self._move_base.send_goal(self.obj_pose) # send goal
rospy.loginfo('Performing approach to 2m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
rospy.loginfo('Waiting for result...')
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle
# else if distance is less than 2m and more than 1.20m
elif (self.tracked_blob.linear.z > 0.0012):
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.001) # set goal to (distance - 1m)
self._move_base.send_goal(self.obj_pose) # send goal
rospy.loginfo('Performing approach to 1m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
rospy.loginfo('Waiting for result...')
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
rospy.loginfo(self._move_base.get_result())
self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle
# else if distance is less than 1m
else:
self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # set goal to final target
self._move_base.send_goal(self.obj_pose) # send goal
rospy.loginfo('Performing final approach. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w))
rospy.loginfo('Waiting for result...')
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
self.moveStartTime = rospy.Time.now()
self.state = FSM_STATES.FSM_SPIN # move to FSM_SPIN
# FSM_SPIN STATE
# =============>
elif (self.state == FSM_STATES.FSM_SPIN):
# Spin
if (rospy.Time.now() < self.moveStartTime + self.moveDuration):
self.speed.linear.x = 0.0
self.speed.angular.z = 0.5
self.state = FSM_STATES.FSM_SPIN
else:
self.state = FSM_STATES.FSM_WAIT_FOR_DROP
# FSM_WAIT_FOR_DROP STATE
# =============>
elif (self.state == FSM_STATES.FSM_WAIT_FOR_DROP):
self.speed.linear.x = 0.0
self.speed.angular.z = 0.0
# Next state transitions
if (self.arm_state.data == 'OBJ_DROPPED'): # if arm signifies that object has been dropped
self.state = FSM_STATES.FSM_MOVE_TO_BASE # change to MOVE_TO_BASE state
else: # if object not dropped
self.state = FSM_STATES.FSM_WAIT_FOR_DROP # loop back
# FSM_MOVE_TO_BASE STATE
# =============>
elif (self.state == FSM_STATES.FSM_MOVE_TO_BASE):
# **TEST** wait for coordinates of base to be passed and then move to base
if (self._move_base.simple_state == 2):
rospy.loginfo("Waiting for base coordinates...")
self.state = FSM_STATES.FSM_MOVE_TO_BASE
else: # if a goal is detected
rospy.loginfo("Moving back to base")
rospy.loginfo("Goal Status: %f" %self._move_base.get_state())
self._move_base.wait_for_result()
if self._move_base.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Goal Reached!')
else:
rospy.loginfo('Goal aborted!')
self.state = FSM_STATES.FSM_NAVIGATE
# Print general info
self.printINFO()
# Publish /arm_bot_base/state
self.state_pub.publish(String(FSM_STATES.__geitem__(self.state)))
# Only publish velocity when no goal is being pursued or expected
if (self.state != FSM_STATES.FSM_NAVIGATE and self.state != FSM_STATES.FSM_MOVE_TO_BASE and self.state != FSM_STATES.FSM_APPROACH_OBJ):
self.speed_pub.publish(self.speed)
############
# getPose(): Returns pose of received blob relative to the camera.
############
def getPose(self, blob):
self.pose = Vector3()
self.pose.x = blob.center.x
self.pose.y = blob.center.y
self.pose.z = blob.center.z
return self.pose
def printINFO(self):
# Print debug info about linear and angular speeds
rospy.loginfo("speedPID: %f" %(self.speed.linear.x))
rospy.loginfo("anglePID: %f" %(self.speed.angular.z))
if(self.blobs_detected):
rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z))
rospy.loginfo("miss_counter = %f" %(self.miss_counter))
else:
rospy.loginfo("No blobs detected")
rospy.loginfo("miss_counter = %f" %(self.miss_counter))
def spin(self):
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.speed_callback()
rate.sleep()
