class Terminal(object):
def __init__(self, name='terminal'):
self.client = SimpleActionClient(resolve_name(name), ExchangeAction)
self.client.wait_for_server()
def __del__(self):
self.close()
def close(self):
del self.client
def send(self, mode, text, handler=None, block=True):
goal = ExchangeGoal()
goal.mode = mode
goal.text = text
self.client.send_goal(goal, feedback_cb=handler)
if block:
self.client.wait_for_result()
def write(self, text):
self.send(PRINT, text)
def prompt(self, text):
self.send(LINE, text)
return self.client.get_result().line
def get(self, text, escape, handler, block=True):
callback = lambda feedback: handler(feedback.key)
self.send(KEY, escape + text, callback, block)
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
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 IntroduceSelf(smach.State):
def __init__(self,
profession=True,
residence=True,
date_of_birth=True,
timeout=120.0,
action_server='/mdr_actions/introduce_self_action_server'):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.profession = profession
self.residence = residence
self.date_of_birth = date_of_birth
self.timeout = timeout
self.introduce_self_client = SimpleActionClient(
action_server, IntroduceSelfAction)
self.introduce_self_client.wait_for_server()
def execute(self, userdata):
goal = IntroduceSelfGoal()
goal.profession = self.profession
goal.residence = self.residence
goal.date_of_birth = self.date_of_birth
rospy.loginfo('Calling introduce_self action')
self.introduce_self_client.send_goal(goal)
duration = rospy.Duration.from_sec(self.timeout)
self.introduce_self_client.wait_for_result(duration)
rospy.loginfo('introduce_self call completed')
res = self.introduce_self_client.get_result()
if res and res.success:
return 'succeeded'
else:
return 'failed'
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 Planning:
def __init__(self):
self.client = SimpleActionClient('/move_base', MoveBaseAction)
self.goal = MoveBaseGoal()
self.current_goal = 'home'
self.goals = {
'home': np.array([0.0, 0.0]),
'pos1': np.array([1.0, 0.0]),
'pos2': np.array([1.0, 1.0]),
}
def sendGoal(self, x_pos, y_pos, yaw):
if abs(x_pos) <= 0.1:
x_pos = 0.0
if abs(y_pos) <= 0.1:
y_pos = 0.0
# quaternion = transformations.quaternion_from_euler(0, 0, yaw)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = x_pos
goal.target_pose.pose.position.y = y_pos
goal.target_pose.pose.position.z = 0.0
# goal.target_pose.pose.orientation.x = quaternion[0]
# goal.target_pose.pose.orientation.y = quaternion[1]
# goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = 1.0 #quaternion[3]
self.client.send_goal(goal)
self.client.wait_for_result()
print('Reached ' + self.current_goal)
print(self.client.get_result())
def computeNewGoal(self):
if self.current_goal == 'pos2':
self.current_goal = 'home'
elif self.current_goal == 'pos1':
self.current_goal = 'pos2'
elif self.current_goal == 'home':
self.current_goal = 'pos1'
else:
self.current_goal = 'home'
self.sendGoal(self.goals[self.current_goal][0],
self.goals[self.current_goal][1], 0)
def loop(self, rate):
self.sendGoal(self.goals['home'][0], self.goals['home'][1], 0)
while not rospy.is_shutdown():
self.computeNewGoal()
rate.sleep()
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'
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...")
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'
class EnterDoor(smach.State):
def __init__(self,
timeout=120.0,
move_forward_server='/mdr_actions/move_forward_server',
movement_duration=15.,
speed=0.1):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.timeout = timeout
self.movement_duration = movement_duration
self.speed = speed
self.move_forward_client = SimpleActionClient(move_forward_server,
MoveForwardAction)
self.move_forward_client.wait_for_server()
self.entered = False
def execute(self, userdata):
goal = MoveForwardGoal()
goal.movement_duration = self.movement_duration
goal.speed = self.speed
self.move_forward_client.send_goal(goal)
timeout_duration = rospy.Duration.from_sec(self.timeout)
self.move_forward_client.wait_for_result(timeout_duration)
result = self.move_forward_client.get_result()
if result and result.success:
return 'succeeded'
else:
return 'failed'
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 Gripper():
LEFT = 'l'
RIGHT = 'r'
def __init__(self, direction):
assert (direction == Gripper.LEFT or direction == Gripper.RIGHT)
self.direction = direction
name_space = '/{0}_gripper_controller/gripper_action'.format(
self.direction)
self.gripper_client = SimpleActionClient(name_space,
GripperCommandAction)
self.gripper_client.wait_for_server()
def open_gripper(self, wait=False):
self.change_state(0.08, wait)
def close_gripper(self, wait=False):
self.change_state(0.0, wait)
def change_state(self, value, wait):
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = value
gripper_goal.command.max_effort = 30.0
self.gripper_client.send_goal(gripper_goal)
if wait:
self.gripper_client.wait_for_result()
if not self.gripper_client.get_result().reached_goal:
time.sleep(1)
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 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 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 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
def move_base(self, offset):
client = SimpleActionClient(
'/position_arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
client.wait_for_server()
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
'move_1_joint', 'move_2_joint', 'move_3_joint'
]
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(0.3)
point.positions = offset.tolist()
# point1 = JointTrajectoryPoint()
# point1.time_from_start = rospy.Duration.from_sec(0.3)
# print point1.time_from_start
# point1.positions = [0, 0, 0]
# point2 = JointTrajectoryPoint()
# point2.time_from_start = rospy.Duration.from_sec(0.6)
# point2.positions = [offset_x, offset_y, offset_z]
goal.trajectory.points.append(point)
# goal.trajectory.points.append(point2)
client.send_goal(goal)
client.wait_for_result()
class RecognizeGenders(smach.State):
def __init__(self, **kwargs):
smach.State.__init__(
self,
outcomes=['succeeded', 'failed'],
input_keys=['image', 'number_of_faces', 'bounding_boxes'])
self.timeout = kwargs.get('timeout', 10)
self.say_topic = kwargs.get('say_topic', '/say')
self.say_pub = rospy.Publisher(self.say_topic, String, queue_size=1)
self.gender_client = SimpleActionClient(
'mdr_actions/gender_recognition_server', GenderRecognitionAction)
self.gender_client.wait_for_server()
def execute(self, userdata):
# Recognize gender
goal = GenderRecognitionGoal()
goal.image = userdata.image
goal.number_of_faces = userdata.number_of_faces
goal.bounding_boxes = userdata.bounding_boxes
self.gender_client.send_goal(goal)
self.gender_client.wait_for_result()
result = self.gender_client.get_result()
men = result.genders.count('man')
women = result.genders.count('woman')
# Say x men and y women
msg = String()
msg.data = 'There are %i men and %i women' % (men, women)
rospy.loginfo(msg.data)
self.say_pub.publish(msg)
return 'succeeded'
class FollowTrajectoryClient(object):
"""
Class to make modules (torso) follow trajectory
"""
def __init__(self, rospy, name, joint_names):
self.rospy =rospy
self.client = SimpleActionClient("%s/follow_joint_trajectory" % name, FollowJointTrajectoryAction)
self.rospy.loginfo("Waiting for %s..." % name)
self.client.wait_for_server()
self.joint_names = joint_names
def move_to(self, positions, duration = 5.0):
if len(self.joint_names) != len(positions):
print("Invalid trajectory position")
return False
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = positions
trajectory.points[0].velocities = [0.0 for _ in positions]
trajectory.points[0].accelerations = [0.0 for _ in positions]
trajectory.points[0].time_from_start = self.rospy.Duration(duration)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
class Gripper:
LEFT = "l"
RIGHT = "r"
def __init__(self, direction):
assert direction == Gripper.LEFT or direction == Gripper.RIGHT
self.direction = direction
name_space = "/{0}_gripper_controller/gripper_action".format(self.direction)
self.gripper_client = SimpleActionClient(name_space, GripperCommandAction)
self.gripper_client.wait_for_server()
def open_gripper(self, wait=False):
self.change_state(0.08, wait)
def close_gripper(self, wait=False):
self.change_state(0.0, wait)
def change_state(self, value, wait):
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = value
gripper_goal.command.max_effort = 30.0
self.gripper_client.send_goal(gripper_goal)
if wait:
self.gripper_client.wait_for_result()
if not self.gripper_client.get_result().reached_goal:
time.sleep(1)
class MoveBase(smach.State):
def __init__(self,
destination_locations,
timeout=120.0,
action_server='/mdr_actions/move_base_safe_server'):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.destination_locations = list(destination_locations)
self.timeout = timeout
self.action_server = action_server
self.client = SimpleActionClient(action_server, MoveBaseSafeAction)
self.client.wait_for_server()
def execute(self, userdata):
goal = MoveBaseSafeGoal()
for i, destination_location in enumerate(self.destination_locations):
goal.destination_location = destination_location
rospy.loginfo('Sending the base to %s' % destination_location)
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration.from_sec(self.timeout))
res = self.client.get_result()
if res and res.success:
rospy.loginfo('Successfully reached %s' % destination_location)
else:
rospy.logerr('Could not reach %s' % destination_location)
return 'failed'
return 'succeeded'
def on_play_2(self):
change_to_controller('position')
pm = SimpleActionClient('/play_motion', PlayMotionAction)
pm.wait_for_server()
pmg = PlayMotionGoal()
pmg.motion_name = 'LBD_2X'
pm.send_goal(pmg)
pm.wait_for_result(rospy.Duration(0.1))
class TriggeredImageTopic():
"""
This nodes provides allows to transform a trigger topic to an image topic using GrabImageAction.
Whenever a signal is published on ~trigger a sensor_msg.msg.Image is published on the output topic.
The interface is restricted to a rosparam for exposure with a low gain
"""
def __init__(self):
self.camera_name = rospy.get_param('~camera_name', '')
self.output_topic_name = rospy.get_param('~triggered_image_topic',
'triggered_images')
if not self.camera_name:
rospy.logwarn(
"No camera name given! Assuming 'pylon_camera_node' as"
" camera name")
self.camera_name = '/pylon_camera_node'
else:
rospy.loginfo('Camera name is: ' + self.camera_name)
self._grab_imgs_rect_ac = SimpleActionClient(
'{}/grab_images_rect'.format(self.camera_name), GrabImagesAction)
if self._grab_imgs_rect_ac.wait_for_server(rospy.Duration(10.0)):
rospy.loginfo('Found action server at '
'{}/grab_images_raw'.format(self.camera_name))
else:
rospy.logerr('Could not connect to action server at '
'{}/grab_images_raw'.format(self.camera_name))
self.pub = rospy.Publisher(self.output_topic_name,
Image,
queue_size=10)
self.subscriber = rospy.Subscriber("~trigger", Empty, self.trigger_cb)
def trigger_cb(self, msg):
grab_imgs_goal = GrabImagesGoal()
grab_imgs_goal.exposure_given = True
grab_imgs_goal.exposure_times = [
rospy.get_param('~exposure_time', 20000.0)
]
grab_imgs_goal.gain_given = True
grab_imgs_goal.gain_values = [0.2]
grab_imgs_goal.gain_auto = False
self._grab_imgs_rect_ac.send_goal(grab_imgs_goal)
self._grab_imgs_rect_ac.wait_for_result(rospy.Duration.from_sec(10.0))
grab_imgs_result = self._grab_imgs_rect_ac.get_result()
rospy.loginfo("Got image")
if len(grab_imgs_result.images) > 0:
rospy.loginfo("publish image")
self.pub.publish(grab_imgs_result.images[0])
def spin(self):
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
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!")
def move(lb, ub, from_location, to_location):
coords = get_coords(to_location)
header = Header(frame_id='/map')
point = Point(**coords)
quat = Quaternion(0, 0, 0, 1)
pose = Pose(point, quat)
move_base_client = SimpleActionClient('move_base', MoveBaseAction)
move_base_client.wait_for_server()
move_base_client.send_goal(MoveBaseGoal(PoseStamped(header, pose)))
move_base_client.wait_for_result()
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 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
class MoveBaseClient(object):
"""
Class to move the fetch base to desired position
"""
def __init__(self, rospy):
# Constants for the class
ROSTOPIC_NAME = "move_base"
# Initialization
self.rospy = rospy
self.client = SimpleActionClient(ROSTOPIC_NAME, MoveBaseAction)
self.rospy.loginfo("Waiting for /move_base ....")
self.client.wait_for_server()
self.rospy.loginfo("Connected to /move_base")
def goto2d(self, x, y, theta = 0.0, frame = "map"):
"""
Structure:
target_pose:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
"""
move_goal = MoveBaseGoal()
move_goal.target_pose.pose.position.x = x
move_goal.target_pose.pose.position.y = y
move_goal.target_pose.pose.orientation.z = sin(theta/2.0)
move_goal.target_pose.pose.orientation.w = cos(theta/2.0)
move_goal.target_pose.header.frame_id = frame
move_goal.target_pose.header.stamp = self.rospy.Time.now()
self.client.send_goal(move_goal)
self.client.wait_for_result()
#self.client.send_goal_and_wait(move_goal,execute_timeout=rospy.Duration(10,0), preempt_timeout=rospy.Duration(20,0))
def goto3d(self):
"""
Function not implemented
"""
pass
class fibonacci_client:
def __init__(self, name):
self.action_client = SimpleActionClient('fibonacci_server',
FibonacciAction)
self.action_client.wait_for_server()
goal = FibonacciGoal(order=20)
self.action_client.send_goal(goal)
self.action_client.wait_for_result()
result = self.action_client.get_result()
rospy.loginfo('[Fibonacci Client] Result: {}'.format(', '.join(
[str(n) for n in result.sequence])))
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 applyEndPosition(self):
rospy.loginfo("Initializing dmp_execution test.")
diction = []
for i in range(0, len(self.plan_resp.plan.points)):
diction.append({
'positions': self.plan_resp.plan.points[i].positions,
'time_from_start': self.plan_resp.plan.times[i]
})
name = 'movement'
d = {
'play_motion': {
'controllers': self.controllers,
'motions': {
name: {
'joints': self.joint_names,
'points': diction
}
}
}
}
# Create params
rospy.set_param('/play_motion/motions/movement/points', diction)
rospy.set_param('/play_motion/motions/movement/joints',
self.joint_names)
#rospy.loginfo("Parameters created")
client = SimpleActionClient("play_motion", PlayMotionAction)
client.wait_for_server()
rospy.loginfo("...connected.")
#rospy.wait_for_message("joint_states", JointState)
#rospy.sleep(3.0)
rospy.loginfo("Initial position...")
goal = PlayMotionGoal()
goal.motion_name = 'movement'
goal.skip_planning = True
#raw_input('Press Enter to execute action\n')
client.send_goal(goal)
client.wait_for_result(rospy.Duration(15.0))
rospy.loginfo("Movement reached.")
rospy.loginfo("Action done")
class GripperController:
def __init__(self, hand):
arm = hand[0]
self.effort = -1.0
self.server = SimpleActionServer(ACTION_NAME % arm,
GripperSequenceAction, self.execute,
False)
self.server.start()
self.sub_client = SimpleActionClient(
"%s_gripper_controller/gripper_action" % arm,
Pr2GripperCommandAction)
#wait for the action servers to come up
rospy.loginfo("[GRIPPER] Waiting for %s controllers" % arm)
self.sub_client.wait_for_server()
rospy.loginfo("[GRIPPER] Got %s controllers" % arm)
self.client = SimpleActionClient(ACTION_NAME % arm,
GripperSequenceAction)
rospy.loginfo("[GRIPPER] Waiting for self client")
self.client.wait_for_server()
rospy.loginfo("[GRIPPER] Got self client")
def send_goal(self, goal):
self.client.send_goal(goal)
def execute(self, goal):
rate = rospy.Rate(10)
while rospy.Time.now() < goal.header.stamp:
rate.sleep()
for position, time in zip(goal.positions, goal.times):
self.change_position(position, False)
rospy.sleep(time)
self.server.set_succeeded(GripperSequenceResult())
self.change_position(position, False, 0.0)
def change_position(self, position, should_wait=True, effort=None):
if effort is None:
effort = self.effort
gg = Pr2GripperCommandGoal()
gg.command.position = position
gg.command.max_effort = effort
self.sub_client.send_goal(gg)
if should_wait:
self.sub_client.wait_for_result()
class execute_plan(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=['success', 'failure', 'preempted'],
input_keys=['already_once_executed', 'plan'],
output_keys=['already_once_executed'])
self.planner_executor_client = SimpleActionClient(
'/task_planning/execute_plan', ExecutePlanAction)
self.planner_executor_client.wait_for_server()
rospy.sleep(0.2)
def execute(self, userdata):
userdata.already_once_executed = True
success = self.execute_plan(userdata.plan)
if not success:
return 'failure'
return 'success'
def execute_plan(self, plan):
goal = ExecutePlanGoal()
goal.plan = plan
self.planner_executor_client.send_goal(goal)
finished = self.planner_executor_client.wait_for_result()
if not finished:
return (False)
res = self.planner_executor_client.get_result()
return res.success
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 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 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 Enter(ScenarioStateBase):
def __init__(self, save_sm_state=False, **kwargs):
ScenarioStateBase.__init__(
self,
'enter_door',
save_sm_state=save_sm_state,
outcomes=['succeeded', 'failed', 'failed_after_retrying'],
output_keys=['enter_door_feedback'])
self.timeout = kwargs.get('timeout', 120.)
self.number_of_retries = kwargs.get('number_of_retries', 0)
self.retry_count = 0
self.enter_door_server = kwargs.get('enter_action_server',
'enter_door_server')
self.enter_action_client = SimpleActionClient(self.enter_door_server,
EnterDoorAction)
self.enter_action_client.wait_for_server(rospy.Duration(10.))
def execute(self, userdata):
goal = EnterDoorGoal()
rospy.loginfo('[ENTER_DOOR] Calling door entering action')
self.say('Entering door')
self.enter_action_client.send_goal(goal)
duration = rospy.Duration.from_sec(self.timeout)
success = self.enter_action_client.wait_for_result(duration)
if success:
rospy.loginfo('Entered successfully')
return 'succeeded'
else:
rospy.logerr('Entering failed')
self.say('Could not enter door')
if self.retry_count == self.number_of_retries:
self.say('Aborting operation')
return 'failed_after_retrying'
return 'failed'
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.')
class Torso:
def __init__(self):
name_space = '/torso_controller/position_joint_action'
self.torso_client = SimpleActionClient(name_space, SingleJointPositionAction)
self.torso_client.wait_for_server()
def move(self, position):
assert(position >= 0.0 and position <= 0.2)
self.position = position
up = SingleJointPositionGoal()
up.position = self.position
up.min_duration = rospy.Duration(2.0)
up.max_velocity = 1.0
self.torso_client.send_goal(up)
self.torso_client.wait_for_result()
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 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.')
class Move_Head:
"""
a class to fins and pick up a object
"""
def __init__(self, text):
#self.task = text#whole message from greeting
self.head_client = SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
self.head_client.wait_for_server()
ar = text.split(':')
cmd = ar[0]
direction = ar[5]
if direction =='left':
self.move_head(1.1, 0.0)
if direction =='right':
self.move_head(-1.1, 0.0)
if direction =='front':
self.move_head(0.0, 0.0)
def move_head(self, pan , tilt):
# Which joints define the head?
head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
# Create a single-point head trajectory with the head_goal as the end-point
head_trajectory = JointTrajectory()
head_trajectory.joint_names = head_joints
head_trajectory.points.append(JointTrajectoryPoint())
head_trajectory.points[0].positions = pan , tilt
head_trajectory.points[0].velocities = [0.0 for i in head_joints]
head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = head_trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
self.head_client.send_goal(head_goal)
# Wait for up to 5 seconds for the motion to complete
self.head_client.wait_for_result(rospy.Duration(2.0))
def main():
rospy.init_node("send_reference")
ref_args = sys.argv[1:]
if not ref_args:
rospy.logwarn("Usage: send_reference JOINT_NAME_1 POSITION_1 [ JOINT_NAME_2 POSITION 2 ... ]")
return 1
ac_joint_traj = SimpleActionClient("/amigo/body/joint_trajectory_action", FollowJointTrajectoryAction)
while not ac_joint_traj.wait_for_server(timeout=rospy.Duration(1.0)) and not rospy.is_shutdown():
rospy.logwarn("Waiting for server...")
if rospy.is_shutdown():
rospy.logwarn("No reference sent")
return 1
rospy.loginfo("Connected to server")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Even items in ref_args are joint_names
joint_names = ref_args[0::2]
# Odd items in ref_args are set points
ref_positions = [float(i) for i in ref_args[1::2]]
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
points = [JointTrajectoryPoint(positions=ref_positions, time_from_start=rospy.Duration(0))]
joint_trajectory = JointTrajectory(joint_names=joint_names, points=points)
goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory, goal_time_tolerance=rospy.Duration(1000))
ac_joint_traj.send_goal(goal)
ac_joint_traj.wait_for_result(rospy.Duration(1000))
res = ac_joint_traj.get_result()
if res.error_string:
rospy.logerr(res.error_string)
def move_gripper():
name_space = "/{0}_gripper_controller/gripper_action".format(self.direction)
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
if self.state:
# The gripper is open. Close it and update its state.
gripper_goal.command.position = 0.0
self.state = Gripper.CLOSED
else:
# The gripper is closed. Open it and update its state.
gripper_goal.command.position = 0.08
self.state = Gripper.OPEN
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
gripper_client.wait_for_result(rospy.Duration(10.0))
self.gui.show_text_in_rviz("Gripper")
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')
class TrajectoryArmController:
def __init__( self, controller_name ):
topic = ''.join( ( controller_name, '/joint_trajectory_action' ) )
self.client = SimpleActionClient( topic, JointTrajectoryAction )
rospy.loginfo( 'waiting for action client: %s'%topic )
self.client.wait_for_server()
def move( self, trajectory ):
# push trajectory goal to ActionServer
goal = JointTrajectoryGoal()
goal.trajectory = trajectory
goal.trajectory.header.stamp = rospy.Time.now()
self.client.send_goal( goal )
self.client.wait_for_result()
if self.client.get_state == GoalStatus.SUCCEEDED:
rospy.logerr( 'failed to move arm to goal:\n %s'%goal )
return False
else:
return True
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 ObjectSoundCollector:
def __init__(self):
self.gripper_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction)
self.gripper_client.wait_for_server()
def close_gripper(self, torque_limit, dynamic_torque_control):
goal = WubbleGripperGoal()
goal.command = WubbleGripperGoal.CLOSE_GRIPPER
goal.torque_limit = torque_limit
goal.dynamic_torque_control = dynamic_torque_control
goal.pressure_upper = 1500.0
goal.pressure_lower = 1300.0
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
def open_gripper(self):
goal = WubbleGripperGoal()
goal.command = WubbleGripperGoal.OPEN_GRIPPER
goal.torque_limit = 0.4
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
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 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 Kill():
REFRESH_RATE = 20
def __init__(self):
self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675]
self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644]
self.v = [0] * len(self.r1)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self._action_name = 'kill'
self._tf = tf.TransformListener()
#initialize base controller
topic_name = '/base_controller/command'
self._base_publisher = rospy.Publisher(topic_name, Twist)
#Initialize the sound controller
self._sound_client = SoundClient()
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.pose = None
self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback)
#initialize this client
self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def marker_callback(self, marker):
if (self.pose is not None):
self.pose = marker.pose
def run(self, goal):
rospy.loginfo('Begin kill')
self.pose = goal.pose
#pose = self._tf.transformPose('/base_link', goal.pose)
self._sound_client.say('Halt!')
# turn to face the marker before opening arms (code repeated later)
r = rospy.Rate(self.REFRESH_RATE)
while(True):
pose = self.pose
if abs(pose.pose.position.y) > .1:
num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / ( 3 * abs(pose.pose.position.y)))
for i in range(num_move_y):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.y = 0
else:
break
# open arms
traj_goal_r = JointTrajectoryGoal()
traj_goal_r.trajectory.joint_names = self.r_joint_names
traj_goal_l = JointTrajectoryGoal()
traj_goal_l.trajectory.joint_names = self.l_joint_names
traj_goal_r.trajectory.points.append(JointTrajectoryPoint(positions=self.r1, velocities = self.v, time_from_start = rospy.Duration(2)))
self.r_traj_action_client.send_goal_and_wait(traj_goal_r, rospy.Duration(3))
traj_goal_l.trajectory.points.append(JointTrajectoryPoint(positions=self.l1, velocities = self.v, time_from_start = rospy.Duration(2)))
self.l_traj_action_client.send_goal_and_wait(traj_goal_l, rospy.Duration(3))
traj_goal_r.trajectory.points[0].positions = self.r2
self.r_traj_action_client.send_goal(traj_goal_r)
traj_goal_l.trajectory.points[0].positions = self.l2
self.l_traj_action_client.send_goal(traj_goal_l)
self._sound_client.say('You have the right to remain silent.')
# Keep a local copy because it will update
#pose = self.pose
#num_move_x = int((pose.pose.position.x - 0.3) * 10 / .1) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
#twist_msg = Twist()
#twist_msg.linear = Vector3(.1, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, 0.0)
#for i in range(num_move_x):
# self._base_publisher.publish(twist_msg)
# r.sleep()
# track the marker as much as we can
while True:
pose = self.pose
# too far away
if abs(pose.pose.position.x > 1.5):
rospy.loginfo('Target out of range: ' + str(pose.pose.position.x))
self._as.set_aborted()
return;
# too far to the side -> rotate
elif abs(pose.pose.position.y) > .1:
num_move_y = int((abs(pose.pose.position.y) - 0.1) * self.REFRESH_RATE / .33) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (3 * abs(pose.pose.position.y)))
for i in range(num_move_y):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.y = 0
#twist_msg = Twist()
#twist_msg.linear = Vector3(0.0, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, pose.pose.position.y / (5 * abs(pose.pose.position.y)))
#self._base_publisher.publish(twist_msg)
# now we are going to move in for the kill, but only until .5 meters away (don't want to run suspect over)
elif pose.pose.position.x > .5:
num_move_x = int((pose.pose.position.x - 0.3) * self.REFRESH_RATE / .1) + 1
#print str(pose.pose.position.x) + ', ' + str(num_move_x)
twist_msg = Twist()
twist_msg.linear = Vector3(.1, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
for i in range(num_move_x):
if pose != self.pose:
break
self._base_publisher.publish(twist_msg)
r.sleep()
pose.pose.position.x = 0
#twist_msg = Twist()
#twist_msg.linear = Vector3(.1, 0.0, 0.0)
#twist_msg.angular = Vector3(0.0, 0.0, 0.0)
#self._base_publisher.publish(twist_msg)
# susupect is within hugging range!!!
else:
break
r.sleep()
# after exiting the loop, we should be ready to capture -> send close arms goal
self._sound_client.say("Anything you say do can and will be used against you in a court of law.")
self.l_traj_action_client.wait_for_result(rospy.Duration(3))
self.r_traj_action_client.wait_for_result(rospy.Duration(3))
traj_goal_r.trajectory.points[0].positions = self.r3
self.r_traj_action_client.send_goal(traj_goal_r)
traj_goal_l.trajectory.points[0].positions = self.l3
self.l_traj_action_client.send_goal(traj_goal_l)
self.l_traj_action_client.wait_for_result(rospy.Duration(3))
self.r_traj_action_client.wait_for_result(rospy.Duration(3))
rospy.loginfo('End kill')
rospy.sleep(20)
self._as.set_succeeded()
class 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 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()
class HERBRobot(Robot):
def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
left_hand_sim, right_hand_sim, left_ft_sim,
head_sim, talker_sim, segway_sim, perception_sim):
from prpy.util import FindCatkinResource
Robot.__init__(self, robot_name='herb')
# Controller setup
self.controller_manager = None
self.controllers_always_on = []
self.full_controller_sim = (left_arm_sim and right_arm_sim and
left_ft_sim and right_ft_sim and
left_hand_sim and right_hand_sim and
head_sim)
if not self.full_controller_sim:
# any non-simulation requires ros and the ros_control stack
import rospy
from ros_control_client_py import (ControllerManagerClient,
JointStateClient)
if not rospy.core.is_initialized():
raise RuntimeError('rospy not initialized. '
'Must call rospy.init_node()')
# update openrave state from /joint_states
self._jointstate_client = JointStateClient(
self, topic_name='/joint_states')
self.controller_manager = ControllerManagerClient()
self.controllers_always_on.append('joint_state_controller')
# Convenience attributes for accessing self components.
self.left_arm = self.GetManipulator('left')
self.right_arm = self.GetManipulator('right')
self.head = self.GetManipulator('head')
self.left_arm.hand = self.left_arm.GetEndEffector()
self.right_arm.hand = self.right_arm.GetEndEffector()
self.left_hand = self.left_arm.hand
self.right_hand = self.right_arm.hand
self.manipulators = [self.left_arm, self.right_arm, self.head]
# Dynamically switch to self-specific subclasses.
prpy.bind_subclass(self.left_arm, WAM, sim=left_arm_sim, namespace='/left')
prpy.bind_subclass(self.right_arm, WAM, sim=right_arm_sim, namespace='/right')
prpy.bind_subclass(self.head, HERBPantilt, sim=head_sim, owd_namespace='/head/owd')
prpy.bind_subclass(self.left_arm.hand, BarrettHand, sim=left_hand_sim, manipulator=self.left_arm,
bhd_namespace='/left', ft_sim=left_ft_sim)
prpy.bind_subclass(self.right_arm.hand, BarrettHand, sim=right_hand_sim, manipulator=self.right_arm,
bhd_namespace='/right', ft_sim=right_ft_sim)
self.base = HerbBase(sim=segway_sim, robot=self)
# Set HERB's acceleration limits. These are not specified in URDF.
accel_limits = self.GetDOFAccelerationLimits()
accel_limits[self.head.GetArmIndices()] = [ 2. ] * self.head.GetArmDOF()
accel_limits[self.left_arm.GetArmIndices()] = [ 2. ] * self.left_arm.GetArmDOF()
accel_limits[self.right_arm.GetArmIndices()] = [ 2. ] * self.right_arm.GetArmDOF()
self.SetDOFAccelerationLimits(accel_limits)
# Determine always-on controllers
# hand controllers
if not left_hand_sim:
self.controllers_always_on.append('left_hand_controller')
if not right_hand_sim:
self.controllers_always_on.append('right_hand_controller')
# force/torque controllers
if not left_ft_sim or not right_ft_sim:
self.controllers_always_on.append('force_torque_sensor_controller')
if not left_ft_sim:
self.controllers_always_on.append('left_tare_controller')
if not right_ft_sim:
self.controllers_always_on.append('right_tare_controller')
# Set default manipulator controllers in sim only
# NOTE: head is ignored until TODO new Schunk head integrated
if left_arm_sim:
self.left_arm.sim_controller = self.AttachController(name=self.left_arm.GetName(),
args='IdealController',
dof_indices=self.left_arm.GetArmIndices(),
affine_dofs=0,
simulated=True)
if right_arm_sim:
self.right_arm.sim_controller = self.AttachController(name=self.right_arm.GetName(),
args='IdealController',
dof_indices=self.right_arm.GetArmIndices(),
affine_dofs=0,
simulated=True)
# load and activate initial controllers
if self.controller_manager is not None:
self.controller_manager.request(
self.controllers_always_on).switch()
# Support for named configurations.
import os.path
self.configurations.add_group('left_arm', self.left_arm.GetArmIndices())
self.configurations.add_group('right_arm', self.right_arm.GetArmIndices())
self.configurations.add_group('head', self.head.GetArmIndices())
self.configurations.add_group('left_hand', self.left_hand.GetIndices())
self.configurations.add_group('right_hand', self.right_hand.GetIndices())
configurations_path = FindCatkinResource('herbpy', 'config/configurations.yaml')
try:
self.configurations.load_yaml(configurations_path)
except IOError as e:
raise ValueError('Failed laoding named configurations from "{:s}".'.format(
configurations_path))
# Hand configurations
from prpy.named_config import ConfigurationLibrary
for hand in [ self.left_hand, self.right_hand ]:
hand.configurations = ConfigurationLibrary()
hand.configurations.add_group('hand', hand.GetIndices())
if isinstance(hand, BarrettHand):
hand_configs_path = FindCatkinResource('herbpy', 'config/barrett_preshapes.yaml')
try:
hand.configurations.load_yaml(hand_configs_path)
except IOError as e:
raise ValueError('Failed loading named hand configurations from "{:s}".'.format(
hand_configs_path))
else:
logger.warning('Unrecognized hand class. Not loading named configurations.')
# Initialize a default planning pipeline.
from prpy.planning import (
FirstSupported,
MethodMask,
Ranked,
Sequence,
)
from prpy.planning import (
CBiRRTPlanner,
CHOMPPlanner,
GreedyIKPlanner,
IKPlanner,
NamedPlanner,
SBPLPlanner,
SnapPlanner,
TSRPlanner,
VectorFieldPlanner
)
# TODO: These should be meta-planners.
self.named_planner = NamedPlanner()
self.ik_planner = IKPlanner()
# Special-purpose planners.
self.snap_planner = SnapPlanner()
self.vectorfield_planner = VectorFieldPlanner()
self.greedyik_planner = GreedyIKPlanner()
# General-purpose planners.
self.cbirrt_planner = CBiRRTPlanner()
# Trajectory optimizer.
try:
from or_trajopt import TrajoptPlanner
self.trajopt_planner = TrajoptPlanner()
except ImportError:
self.trajopt_planner = None
logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt'
' package in your workspace and built?')
try:
self.chomp_planner = CHOMPPlanner()
except UnsupportedPlanningError:
self.chomp_planner = None
logger.warning('Failed loading the CHOMP module. Is the or_cdchomp'
' package in your workspace and built?')
if not (self.trajopt_planner or self.chomp_planner):
raise PrPyException('Unable to load both CHOMP and TrajOpt. At'
' least one of these packages is required.')
actual_planner = Sequence(
# First, try the straight-line trajectory.
self.snap_planner,
# Then, try a few simple (and fast!) heuristics.
self.vectorfield_planner,
#self.greedyik_planner,
# Next, try a trajectory optimizer.
self.trajopt_planner or self.chomp_planner
)
self.planner = FirstSupported(
Sequence(actual_planner,
TSRPlanner(delegate_planner=actual_planner),
self.cbirrt_planner),
# Special purpose meta-planner.
NamedPlanner(delegate_planner=actual_planner),
)
from prpy.planning.retimer import HauserParabolicSmoother
self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True)
self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False,
blend_iterations=5, blend_radius=0.4)
self.simplifier = None
# Base planning
from prpy.util import FindCatkinResource
planner_parameters_path = FindCatkinResource('herbpy', 'config/base_planner_parameters.yaml')
self.sbpl_planner = SBPLPlanner()
try:
with open(planner_parameters_path, 'rb') as config_file:
import yaml
params_yaml = yaml.load(config_file)
self.sbpl_planner.SetPlannerParameters(params_yaml)
except IOError as e:
raise ValueError('Failed loading base planner parameters from "{:s}".'.format(
planner_parameters_path))
self.base_planner = self.sbpl_planner
# Create action library
from prpy.action import ActionLibrary
self.actions = ActionLibrary()
# Register default actions and TSRs
import herbpy.action
import herbpy.tsr
# Setting necessary sim flags
self.talker_simulated = talker_sim
self.segway_sim = segway_sim
# Set up perception
self.detector=None
if perception_sim:
from prpy.perception import SimulatedPerceptionModule
self.detector = SimulatedPerceptionModule()
else:
from prpy.perception import ApriltagsModule
try:
kinbody_path = prpy.util.FindCatkinResource('pr_ordata',
'data/objects')
marker_data_path = prpy.util.FindCatkinResource('pr_ordata',
'data/objects/tag_data.json')
self.detector = ApriltagsModule(marker_topic='/apriltags_kinect2/marker_array',
marker_data_path=marker_data_path,
kinbody_path=kinbody_path,
detection_frame='head/kinect2_rgb_optical_frame',
destination_frame='herb_base',
reference_link=self.GetLink('/herb_base'))
except IOError as e:
logger.warning('Failed to find required resource path. ' \
'pr_ordata package cannot be found. ' \
'Perception detector will not be loaded.' \
'\n{}'.format(e))
if not self.talker_simulated:
# Initialize herbpy ROS Node
import rospy
if not rospy.core.is_initialized():
raise RuntimeError('rospy not initialized. '
'Must call rospy.init_node()')
import talker.msg
from actionlib import SimpleActionClient
self._say_action_client = SimpleActionClient('say', talker.msg.SayAction)
def CloneBindings(self, parent):
from prpy import Cloned
super(HERBRobot, self).CloneBindings(parent)
self.left_arm = Cloned(parent.left_arm)
self.right_arm = Cloned(parent.right_arm)
self.head = Cloned(parent.head)
self.left_arm.hand = Cloned(parent.left_arm.GetEndEffector())
self.right_arm.hand = Cloned(parent.right_arm.GetEndEffector())
self.left_hand = self.left_arm.hand
self.right_hand = self.right_arm.hand
self.manipulators = [ self.left_arm, self.right_arm, self.head ]
self.planner = parent.planner
self.base_planner = parent.base_planner
def _ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01,
**kwargs):
if defer is not False:
raise RuntimeError('defer functionality was deprecated in '
'personalrobotics/prpy#278')
# Don't execute trajectories that don't have at least one waypoint.
if traj.GetNumWaypoints() <= 0:
raise ValueError('Trajectory must contain at least one waypoint.')
# Check if this trajectory contains both affine and joint DOFs
cspec = traj.GetConfigurationSpecification()
needs_base = prpy.util.HasAffineDOFs(cspec)
needs_joints = prpy.util.HasJointDOFs(cspec)
if needs_base and needs_joints:
raise ValueError('Trajectories with affine and joint DOFs are not supported')
# Check that the current configuration of the robot matches the
# initial configuration specified by the trajectory.
if not prpy.util.IsAtTrajectoryStart(self, traj):
raise TrajectoryNotExecutable(
'Trajectory started from different configuration than robot.')
# If there was only one waypoint, at this point we are done!
if traj.GetNumWaypoints() == 1:
return traj
# Verify that the trajectory is timed by checking whether the first
# waypoint has a valid deltatime value.
if not prpy.util.IsTimedTrajectory(traj):
raise ValueError('Trajectory cannot be executed, it is not timed.')
# Verify that the trajectory has non-zero duration.
if traj.GetDuration() <= 0.0:
logger.warning('Executing zero-length trajectory. Please update the'
' function that produced this trajectory to return a'
' single-waypoint trajectory.', FutureWarning)
traj_manipulators = self.GetTrajectoryManipulators(traj)
controllers_manip = []
active_controllers = []
if self.head in traj_manipulators:
# TODO head after Schunk integration
if len(traj_manipulators) == 1:
raise NotImplementedError('The head is currently disabled under ros_control.')
else:
logger.warning('The head is currently disabled under ros_control.')
# logic to determine which controllers are needed
if (self.right_arm in traj_manipulators and
not self.right_arm.IsSimulated() and
self.left_arm in traj_manipulators and
not self.left_arm.IsSimulated()):
controllers_manip.append('bimanual_trajectory_controller')
else:
if self.right_arm in traj_manipulators:
if not self.right_arm.IsSimulated():
controllers_manip.append('right_trajectory_controller')
else:
active_controllers.append(self.right_arm.sim_controller)
if self.left_arm in traj_manipulators:
if not self.left_arm.IsSimulated():
controllers_manip.append('left_trajectory_controller')
else:
active_controllers.append(self.left_arm.sim_controller)
# load and activate controllers
if not self.full_controller_sim:
self.controller_manager.request(controllers_manip).switch()
# repeat logic and actually construct controller clients
# now that we've activated them on the robot
if 'bimanual_trajectory_controller' in controllers_manip:
joints = []
joints.extend(self.right_arm.GetJointNames())
joints.extend(self.left_arm.GetJointNames())
active_controllers.append(
RewdOrTrajectoryController(self, '',
'bimanual_trajectory_controller',
joints))
else:
if 'right_trajectory_controller' in controllers_manip:
active_controllers.append(
RewdOrTrajectoryController(self, '',
'right_trajectory_controller',
self.right_arm.GetJointNames()))
if 'left_trajectory_controller' in controllers_manip:
active_controllers.append(
RewdOrTrajectoryController(self, '',
'left_trajectory_controller',
self.left_arm.GetJointNames()))
if needs_base:
if (hasattr(self, 'base') and hasattr(self.base, 'controller') and
self.base.controller is not None):
active_controllers.append(self.base.controller)
else:
logger.warning(
'Trajectory includes the base, but no base controller is'
' available. Is self.base.controller set?')
for controller in active_controllers:
controller.SetPath(traj)
prpy.util.WaitForControllers(active_controllers, timeout=timeout)
return traj
def ExecuteTrajectory(self, traj, *args, **kwargs):
# from prpy.exceptions import TrajectoryAborted
value = self._ExecuteTrajectory(traj, *args, **kwargs)
# TODO meaningful to do this check here?
# if so can be done on value future
#
# for manipulator in active_manipulators:
# status = manipulator.GetTrajectoryStatus()
# if status == 'aborted':
# raise TrajectoryAborted()
return value
# Inherit docstring from the parent class.
ExecuteTrajectory.__doc__ = Robot.ExecuteTrajectory.__doc__
def SetStiffness(self, stiffness, manip=None):
"""Set the stiffness of HERB's arms and head.
Stiffness False/0 is gravity compensation and stiffness True/(>0) is position
control.
@param stiffness boolean or numeric value 0.0 to 1.0
"""
if (isinstance(stiffness, numbers.Number) and
not (0 <= stiffness and stiffness <= 1)):
raise Exception('Stiffness must be boolean or numeric in the range [0, 1];'
'got {}.'.format(stiffness))
# TODO head after Schunk integration
if manip is self.head:
raise NotImplementedError('Head immobilized under ros_control, SetStiffness not available.')
new_manip_controllers = []
if stiffness:
if not self.left_arm.IsSimulated() and (manip is None or manip is self.left_arm):
new_manip_controllers.append('left_joint_group_position_controller')
if not self.right_arm.IsSimulated() and (manip is None or manip is self.right_arm):
new_manip_controllers.append('right_joint_group_position_controller')
else:
if not self.left_arm.IsSimulated() and (manip is None or manip is self.left_arm):
new_manip_controllers.append(
'left_gravity_compensation_controller')
if not self.right_arm.IsSimulated() and (manip is None or manip is self.right_arm):
new_manip_controllers.append(
'right_gravity_compensation_controller')
if not self.full_controller_sim:
self.controller_manager.request(new_manip_controllers).switch()
def Say(self, words, block=True):
"""Speak 'words' using talker action service or espeak locally in simulation"""
if self.talker_simulated:
import subprocess
try:
proc = subprocess.Popen(['espeak', '-s', '160', '"{0}"'.format(words)])
if block:
proc.wait()
except OSError as e:
logger.error('Unable to speak. Make sure "espeak" is installed locally.\n%s' % str(e))
else:
import talker.msg
goal = talker.msg.SayGoal(text=words)
self._say_action_client.send_goal(goal)
if block:
self._say_action_client.wait_for_result()
class Pr2LookAtFace(Action):
def __init__(self):
super(Pr2LookAtFace, self).__init__()
self._client = SimpleActionClient('face_detector_action',FaceDetectorAction)
self._head_client = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
self._timer = None
self._executing = False
self._pending_face_goal = False
self._pending_head_goal = False
def set_duration(self, duration):
duration = max(duration, 1.5)
super(Pr2LookAtFace, self).set_duration(duration)
def to_string(self):
return 'look_at_face()'
def execute(self):
super(Pr2LookAtFace, self).execute()
print('Pr2LookAtFace.execute() %d' % self.get_duration())
# delay execution to not run nested in the current stack context
self._timer = rospy.Timer(rospy.Duration.from_sec(0.001), self._execute, oneshot=True)
def _execute(self, event):
self._executing = True
self._timer = rospy.Timer(rospy.Duration.from_sec(self.get_duration()), self._preempt, oneshot=True)
hgoal = None
connected = self._client.wait_for_server(rospy.Duration(1.0))
if connected:
while self._executing:
fgoal = FaceDetectorGoal()
self._pending_face_goal = True
self._client.send_goal(fgoal)
self._client.wait_for_result()
self._pending_face_goal = False
f = self._client.get_result()
if len(f.face_positions) > 0:
closest = -1
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:
hgoal = PointHeadGoal()
hgoal.target.header.frame_id = f.face_positions[closest].header.frame_id
hgoal.target.point.x = f.face_positions[closest].pos.x
hgoal.target.point.y = f.face_positions[closest].pos.y
hgoal.target.point.z = f.face_positions[closest].pos.z
hgoal.min_duration = rospy.Duration(1.0)
if self._executing:
hconnected = self._head_client.wait_for_server(rospy.Duration(1.0))
if hconnected and self._executing:
self._pending_head_goal = True
self._head_client.send_goal(hgoal)
# self._head_client.wait_for_result()
# self._pending_head_goal = False
else:
hgoal = PointHeadGoal()
hgoal.target.header.frame_id = "base_footprint";
hgoal.target.point.x = 2.0
hgoal.target.point.y = -2.0
hgoal.target.point.z = 1.2
hgoal.min_duration = rospy.Duration(1.0)
if self._executing:
hconnected = self._head_client.wait_for_server(rospy.Duration(1.0))
if hconnected and self._executing:
self._pending_head_goal = True
self._head_client.send_goal(hgoal)
self._head_client.wait_for_result()
self._pending_head_goal = False
if self._executing:
time.sleep(1.0)
self._finished_finding_face()
def _preempt(self, event):
self._executing = False
if self._pending_face_goal:
self._client.cancel_goal()
if self._pending_head_goal:
self._head_client.cancel_goal()
self._execute_finished()
def _finished_finding_face(self):
if self._executing:
self._executing = False
self._timer.shutdown()
self._execute_finished()
class LiftObjectActionServer:
def __init__(self):
self.get_grasp_status_srv = rospy.ServiceProxy("/wubble_grasp_status", GraspStatus)
self.start_audio_recording_srv = rospy.ServiceProxy("/audio_dump/start_audio_recording", StartAudioRecording)
self.stop_audio_recording_srv = rospy.ServiceProxy("/audio_dump/stop_audio_recording", StopAudioRecording)
self.posture_controller = SimpleActionClient("/wubble_gripper_grasp_action", GraspHandPostureExecutionAction)
self.move_arm_client = SimpleActionClient("/move_left_arm", MoveArmAction)
self.attached_object_pub = rospy.Publisher("/attached_collision_object", AttachedCollisionObject)
self.action_server = SimpleActionServer(
ACTION_NAME, LiftObjectAction, execute_cb=self.lift_object, auto_start=False
)
def initialize(self):
rospy.loginfo("%s: waiting for wubble_grasp_status service" % ACTION_NAME)
rospy.wait_for_service("/wubble_grasp_status")
rospy.loginfo("%s: connected to wubble_grasp_status service" % ACTION_NAME)
rospy.loginfo("%s: waiting for wubble_gripper_grasp_action" % ACTION_NAME)
self.posture_controller.wait_for_server()
rospy.loginfo("%s: connected to wubble_gripper_grasp_action" % ACTION_NAME)
rospy.loginfo("%s: waiting for audio_dump/start_audio_recording service" % ACTION_NAME)
rospy.wait_for_service("audio_dump/start_audio_recording")
rospy.loginfo("%s: connected to audio_dump/start_audio_recording service" % ACTION_NAME)
rospy.loginfo("%s: waiting for audio_dump/stop_audio_recording service" % ACTION_NAME)
rospy.wait_for_service("audio_dump/stop_audio_recording")
rospy.loginfo("%s: connected to audio_dump/stop_audio_recording service" % ACTION_NAME)
rospy.loginfo("%s: waiting for move_left_arm action server" % ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo("%s: connected to move_left_arm action server" % ACTION_NAME)
self.action_server.start()
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
def lift_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo("%s: preempted" % ACTION_NAME)
self.action_server.set_preempted()
collision_support_surface_name = goal.collision_support_surface_name
# disable collisions between grasped object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
# disable collisions between gripper and table
collision_operation2 = CollisionOperation()
collision_operation2.object1 = GRIPPER_GROUP_NAME
collision_operation2.object2 = collision_support_surface_name
collision_operation2.operation = CollisionOperation.DISABLE
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
gripper_paddings = [LinkPadding(l, 0.0) for l in GRIPPER_LINKS]
# this is a hack to make arm lift an object faster
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.REMOVE
obj.object.id = collision_support_surface_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
joint_names = current_state.joint_names
joint_positions = current_state.actual.positions
start_angles = [joint_positions[joint_names.index(jn)] for jn in ARM_JOINTS]
start_angles[0] = start_angles[0] - 0.3 # move shoulder up a bit
if not move_arm_joint_goal(
self.move_arm_client,
ARM_JOINTS,
start_angles,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations,
):
self.action_server.set_aborted()
return
self.start_audio_recording_srv(InfomaxAction.LIFT, goal.category_id)
if move_arm_joint_goal(
self.move_arm_client,
ARM_JOINTS,
LIFT_POSITION,
link_padding=gripper_paddings,
collision_operations=ordered_collision_operations,
):
# check if are still holding an object after lift is done
grasp_status = self.get_grasp_status_srv()
# if the object is still in hand after lift is done we are good
if grasp_status.is_hand_occupied:
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(LiftObjectResult(sound_msg.recorded_sound))
return
self.stop_audio_recording_srv(False)
rospy.logerr("%s: attempted lift failed" % ACTION_NAME)
self.action_server.set_aborted()
return
class PointArmService():
def __init__(self):
rospy.init_node(NAME + 'server' , anonymous=True)
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
# self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
# self.move_client.wait_for_server()
self.tf = tf.TransformListener()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
self.service = rospy.Service('point_arm_service', ArmPointService, self.handle_point)
rospy.loginfo("%s: Ready for pointing", NAME)
#handles receiving a PointStamped. Not handling preempts - does client do this? remember to check code
def handle_point(self, req):
success = False
#print info about request
rospy.loginfo("%s: recieved position %s %s %s in %s", \
NAME, req.position.point.x, req.position.point.y, req.position.point.z, req.position.header.frame_id);
if self.point_at(req.position.header.frame_id, req.position.point.x, req.position.point.y, req.position.point.z):
success = True
return success
#(almost) blatent copy / past from wubble_head_action.py. Err, it's going to the wrong position
def transform_target_point(self, point):
# Cannot use arm_shoulder_pan_link & arm_shoulder_tilt_link as target frame_id
# because the angles used by the low-level motors are absolute, and fixed to
# the arm_base_link coordinate (not relative to the coordinates of pan & tilt)
target_frame = 'arm_base_link'
#rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z)
# Wait for tf info (time-out in 5 seconds)
self.tf.waitForTransform(target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(2.0))
# Transform target point & retrieve the pan angle
pan_target = self.tf.transformPoint(target_frame, point)
pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)
#rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \
# NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle)
# Transform target point & retrieve the tilt angle
tilt_target = self.tf.transformPoint(target_frame, point)
tilt_angle = math.atan2(tilt_target.point.z,
math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))
#rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \
# NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle)
return [pan_angle, tilt_angle]
#point! modified move_arm + reach_at from move_arm_demo
def point_at(self, frame_id, x, y, z):
goal = SmartArmGoal()
goal.target_point = PointStamped()
goal.target_point.header.frame_id = frame_id
goal.target_point.point.x = x
goal.target_point.point.y = y
goal.target_point.point.z = z
self.move_gripper (-2,2)
target_shoulder = list()
target_shoulder = self.transform_target_point(goal.target_point)
if (target_shoulder[0] < 1.2 and target_shoulder[0] > -1.2):
#bend arm a bit
goal.target_joints = [target_shoulder[0], target_shoulder[1] + .2, .3, .5]
# Sends the goal to the action server.
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result(rospy.Duration(2.0))
#straighten arm
goal.target_joints = [target_shoulder[0], target_shoulder[1], 0, 0]
# Sends the goal to the action server.
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
# Return result
#something broke here, returning True fix is temporary
return self.arm_client.get_result()
#motors are too slow in the demo to actually wait for arm to go to cobra position before moving
self.go_to_cobra()
rospy.sleep(2)
position = Point(x, y, z)
orientation = Quaternion(w=1.0)
# self.move_to('/map', position, orientation, 99)
return self.point_at(frame_id, x, y, z)
#I stole this from somewhere
# goal = ErraticBaseGoal()
# goal.target_pose.header.stamp = rospy.Time.now()
# goal.target_pose.header.frame_id = frame_id
# goal.target_pose.pose.position = position
# goal.target_pose.pose.orientation = orientation
# goal.vicinity_range = vicinity
# self.move_client.send_goal(goal)
# self.move_client.wait_for_result()
def go_to_cobra(self):
goal = SmartArmGoal()
goal.target_point = PointStamped()
goal.target_point.header.frame_id = 'arm_base_link'
goal.target_point.point.x = 0
goal.target_point.point.y = 0
goal.target_point.point.z = 0
goal.target_joints = [0.0, 1.972222, -1.972222, 0.0]
self.arm_client.send_goal(goal)
self.arm_client.wait_for_result()
return self.arm_client.get_result()
#stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks
#would be cool if could point with a single claw rather than both (or with 'thumb'?)
def move_gripper(self, left_finger, right_finger):
success = False
goal = SmartArmGripperGoal()
goal.target_joints = [left_finger, right_finger]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_result()
if self.gripper_client.get_result():
success = True
return success
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 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()