class CheckScheduler(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.topic = rospy.get_param("~topic")
rospy.loginfo("Waiting for message type of '%s' ..." % self.topic)
self.ttype = rostopic.get_topic_class(self.topic, True)[0]
rospy.loginfo("... got topic type")
self.rate = rospy.Rate(.02)
self.ok = True
rospy.loginfo("Starting email client ...")
self.client = SimpleActionClient("strands_emails", SendEmailAction)
self.client.wait_for_server()
rospy.loginfo("... done")
self.message = SendEmailGoal()
self.message.to_address = "*****@*****.**"
self.message.subject = "%s stopped working" % self.topic
self.text = "The topic '"+self.topic+"' is not published any more. Last published at %s."
rospy.loginfo("... all done")
def spin(self):
while not rospy.is_shutdown():
try:
rospy.wait_for_message(self.topic, self.ttype, timeout=60.)
except rospy.ROSException:
if self.ok:
rospy.logerr("%s is not published" % self.topic)
self.message.text = self.text % str(time.asctime(time.localtime(time.time())))
# print self.message
self.client.send_goal(self.message)
self.ok = False
else:
self.ok = True
self.rate.sleep()
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 NavigateToMovingGoalState(State):
"""
Moves the robot to a moving goal in the map frame.
"""
def __init__(self, goal): # type: (Callable[[], PoseStamped]) -> None
State.__init__(self, outcomes=['ok', 'err'])
self.goal = goal
self.client = SimpleActionClient('move_base', MoveBaseAction)
def execute(self, ud):
from geometry_msgs.msg import Twist
twist_publisher = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist)
for _ in range(10):
t = Twist()
t.linear.z = -1
twist_publisher.publish(t)
rospy.sleep(0.1)
self.client.wait_for_server()
start_time = rospy.get_time()
while True:
pose = self.goal()
if pose is None:
return 'err'
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position = pose.pose.position
goal.target_pose.pose.orientation = pose.pose.orientation
self.client.send_goal(goal)
if self.client.wait_for_result(timeout=rospy.Duration(1)):
break
if rospy.get_time() - start_time > 30:
return 'ok'
return 'ok'
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 StationHold(HandlerBase):
alarm_name = 'station-hold'
def __init__(self):
self.move_client = SimpleActionClient('/move_to', MoveAction)
self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect)
self.broadcaster = AlarmBroadcaster(self.alarm_name)
def _client_cb(self, terminal_state, result):
if terminal_state != 3:
rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format(
TerminalState.to_string(terminal_state), result))
return
try:
self.change_wrench('autonomous')
rospy.loginfo('Now station holding')
self.broadcaster.clear_alarm()
except rospy.ServiceException as e:
rospy.logwarn('Station hold changing wrench failed: {}'.format(e))
def raised(self, alarm):
rospy.loginfo("Attempting to station hold")
station_hold_goal = MoveGoal()
station_hold_goal.move_type = 'hold'
self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb)
def cleared(self, alarm):
# When cleared, do nothing and just wait for new goal / custom wrench
pass
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'
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 HeadClient():
def __init__(self):
name_space = '/head_traj_controller/point_head_action'
self.head_client = SimpleActionClient(name_space, PointHeadAction)
self.head_client.wait_for_server()
self.hor_pos = 0.0
self.vert_pos = 0.0
def move_head_vert (self, vert_val):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'torso_lift_link'
head_goal.max_velocity = .3
self.vert_pos = vert_val/50.0 - 1
head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos)
self.head_client.send_goal(head_goal)
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def move_head_hor (self, hor_val):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'torso_lift_link'
head_goal.max_velocity = .3
self.hor_pos = -(hor_val/20.0 - 2.5)
head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos)
self.head_client.send_goal(head_goal)
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
class FindInteractant(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=FindInteractantAction,
execute_cb=self.execute_cb,
auto_start=False
)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person", TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Finding interactant '%s'" % (goal.interactant_id,))
self.start_client.send_goal(TrackPersonGoal(id=goal.id, interactant_id=goal.interactant_id, no_turn=True))
res = FindInteractantResult()
res.result.append(ActionResult(cond="found_interactant__"+goal.interactant_id+"__"+goal.id, truth_value=True))
res.result.append(ActionResult(cond="free_interactant_id__"+goal.interactant_id, truth_value=False))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)
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 TerminateInteraction(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=TerminateInteractionAction,
execute_cb=self.execute_cb,
auto_start=False
)
rospy.loginfo("Creating tracker client")
self.stop_client = SimpleActionClient("/stop_tracking_person", TrackPersonAction)
self.stop_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Disengaging human '%s'" % (goal.id,))
self.stop_client.send_goal(TrackPersonGoal())
res = TerminateInteractionResult()
res.result.append(ActionResult(cond="engaged__"+goal.interactant_id+"__"+goal.text, truth_value=False))
res.result.append(ActionResult(cond="said__"+goal.id+"__"+goal.text, truth_value=False))
res.result.append(ActionResult(cond="found_interactant__"+goal.interactant_id+"__"+goal.id, truth_value=False))
res.result.append(ActionResult(cond="human_exists__"+goal.id, truth_value=False))
res.result.append(ActionResult(cond="free_interactant_id__"+goal.interactant_id, truth_value=True))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)
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 Server(object):
def __init__(self, name):
rospy.loginfo("Starting '{test}'.".format(test=name))
self.client = SimpleActionClient("/topo_nav_node", TopoNavAction)
self.client.wait_for_server()
with open(rospy.get_param("~goals_yaml"), "r") as f:
self.goals = yaml.load(f)
print self.goals
self.cnt = {}
self.active_id = -1
self.sub = rospy.Subscriber("/ar_pose_marker", ARMarkers, self.pose_cb)
rospy.loginfo("Started '{test}'.".format(test=name))
def pose_cb(self, msg):
ids = []
for m in msg.markers:
ids.append(m.id)
if m.id in self.cnt:
self.cnt[m.id] += 1
else:
self.cnt[m.id] = 1
print self.cnt
for k in self.cnt.keys():
if k not in ids:
self.cnt[k] = 0
if self.cnt[k] > 5:
if self.active_id != k:
print "Going to:", self.goals[k]
self.client.send_goal(TopoNavGoal(self.goals[k]))
self.active_id = k
self.cnt[k] = 0
class StationHold(HandlerBase):
alarm_name = 'station-hold'
def __init__(self):
self.move_client = SimpleActionClient('/move_to', MoveAction)
self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect)
self.broadcaster = AlarmBroadcaster(self.alarm_name)
def _client_cb(self, terminal_state, result):
if terminal_state != 3:
rospy.logwarn(
'Station hold goal failed (Status={}, Result={})'.format(
TerminalState.to_string(terminal_state), result))
return
try:
self.change_wrench('autonomous')
rospy.loginfo('Now station holding')
self.broadcaster.clear_alarm()
except rospy.ServiceException as e:
rospy.logwarn('Station hold changing wrench failed: {}'.format(e))
def raised(self, alarm):
rospy.loginfo("Attempting to station hold")
station_hold_goal = MoveGoal()
station_hold_goal.move_type = 'hold'
self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb)
def cleared(self, alarm):
# When cleared, do nothing and just wait for new goal / custom wrench
pass
class Move(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['wp_reached', 'wp_not_reached', 'test_finished'],
input_keys=['move_waypoints', 'move_target_wp', 'move_wp_str'],
output_keys=['move_target_wp'])
self.move_base_client = SimpleActionClient('move_base', MoveBaseAction)
self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True)
def execute(self, ud):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = ud.move_waypoints[ud.move_target_wp]
goal.target_pose.header.stamp = rospy.Time.now()
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result()
status = self.move_base_client.get_state()
tts_msg = String()
if status == GoalStatus.SUCCEEDED:
tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "."
self.tts_pub.publish(tts_msg)
if ud.move_target_wp == 0:
return 'test_finished'
else:
ud.move_target_wp -= 1
return 'succeeded'
else:
return 'wp_not_reached'
def _exec_action(action_client: SimpleActionClient, goal, stopping_event: EventBase = None):
"""Executes goal with specified action client and optional event that stops action execution when triggered.
This is a private method.
:param action_client: Action client to use.
:param goal: Action goal to execute.
:param stopping_event: Optional event that stops goal execution when triggered.
:return: None if stopping_event is None. True if goal execution is stopped because of triggered stopping_event,
False otherwise.
"""
if stopping_event is None:
action_client.send_goal(goal)
action_client.wait_for_result()
return
stopping_event.start_listening()
action_client.send_goal(goal,
active_cb=None,
feedback_cb=None,
done_cb=None)
check_rate = rospy.Rate(100)
while True:
goal_state = action_client.get_state()
if goal_state == SimpleGoalState.DONE:
stopping_event.stop_listening()
return False
if stopping_event.is_triggered():
action_client.cancel_goal()
return True
check_rate.sleep()
class TrajectoryExecution:
def __init__(self, side_prefix):
traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints)
self.action_server.start()
self.has_goal = False
def move_to_joints(self, traj_goal):
rospy.loginfo('Receieved a trajectory execution request.')
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
self.traj_action_client.send_goal(traj_goal)
rospy.sleep(1)
is_terminated = False
while(not is_terminated):
action_state = self.traj_action_client.get_state()
if (action_state == GoalStatus.SUCCEEDED):
self.action_server.set_succeeded()
is_terminated = True
elif (action_state == GoalStatus.ABORTED):
self.action_server.set_aborted()
is_terminated = True
elif (action_state == GoalStatus.PREEMPTED):
self.action_server.set_preempted()
is_terminated = True
rospy.loginfo('Trajectory completed.')
class 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 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 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 Speak(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.texts = {"hello": "Hello, I am pepper!"}
self.predicate = rospy.get_param("~predicate", "said")
rospy.loginfo("Starting dialogue client")
self.client = SimpleActionClient("/dialogue_start", dialogueAction)
rospy.loginfo("Waiting for dialogue client")
self.client.wait_for_server()
rospy.loginfo("Dialogue client started")
self.pub = rospy.Publisher("/animated_speech", String, queue_size=10)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=SpeakAction,
execute_cb=self.execute_cb,
auto_start=False)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person",
TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
# self.pub.publish(self.texts[goal.text])
# rospy.sleep(3)
self.start_client.send_goal(TrackPersonGoal(id=goal.id))
self.client.send_goal(dialogueGoal(userID=int(goal.id.split("_")[1])))
res = SpeakResult()
res.result.append(
ActionResult(cond=self.predicate + "__" + goal.id + "__" +
goal.text,
truth_value=ActionResult.TRUE))
self._ps.set_succeeded(res)
def move_base_client():
client = SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
# line - 3, -1, -math.pi/2
theta = eval(sys.argv[3])
quat = quaternion_from_euler(0, 0, theta)
goal.target_pose.pose.position.x = float(sys.argv[1])
goal.target_pose.pose.position.y = float(sys.argv[2])
goal.target_pose.pose.orientation.x = quat[0]
goal.target_pose.pose.orientation.y = quat[1]
goal.target_pose.pose.orientation.z = quat[2]
goal.target_pose.pose.orientation.w = quat[3]
client.send_goal(goal)
wait = client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
return client.get_result()
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 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'
def lookAtLocation(userdata):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "base_link"
# Iterate until we find the object we are searching for
index_of_object = 0
while index_of_object < 10:
if userdata.object_found.object_list[index_of_object].name == userdata.object_to_search_for:
break;
index_of_object += 1
head_goal.target.point.x = userdata.object_found.object_list[index_of_object].pose.position.x
head_goal.target.point.y = userdata.object_found.object_list[index_of_object].pose.position.y
head_goal.target.point.z = userdata.object_found.object_list[index_of_object].pose.position.z
head_goal.pointing_frame = "stereo_link"
head_goal.pointing_axis.x = 1.0
head_goal.pointing_axis.y = 0.0
head_goal.pointing_axis.z = 0.0
head_goal.max_velocity = 1.5
head_goal.min_duration.secs = 1.5
client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction)
client.wait_for_server(rospy.Duration(0.5))
client.send_goal(head_goal)
return succeeded
class ArmController:
def __init__(self, arm):
self.client = SimpleActionClient(
"%s_arm_controller/joint_trajectory_action" % arm,
JointTrajectoryAction)
#wait for the action servers to come up
rospy.loginfo("[ARM] Waiting for %s controller" % arm)
self.client.wait_for_server()
rospy.loginfo("[ARM] Got %s controller" % arm)
def start_trajectory(self, trajectory, set_time_stamp=True, wait=True):
"""Creates an action from the trajectory and sends it to the server"""
goal = JointTrajectoryGoal()
goal.trajectory = trajectory
if set_time_stamp:
goal.trajectory.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
if wait:
self.wait()
def wait(self):
self.client.wait_for_result()
def is_done(self):
return self.client.get_state() > SimpleGoalState.ACTIVE
class 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'
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'
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
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 ExplorationController:
def __init__(self):
rospy.wait_for_service('get_exploration_path', timeout=2)
self._plan_service = rospy.ServiceProxy('get_exploration_path',
GetRobotTrajectory)
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
def run(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
self.run_once()
r.sleep()
def run_once(self):
global count
if (count >= 20
or self._move_base.get_state() == GoalStatus.SUCCEEDED):
path = self._plan_service().trajectory
poses = path.poses
if not path.poses:
rospy.loginfo('No frontiers left.')
return
rospy.loginfo('Moving to frontier...')
self.move_to_pose(poses[-1])
count = 0
else:
count += 1
def move_to_pose(self, pose_stamped, timeout=20.0):
goal = MoveBaseGoal()
goal.target_pose = pose_stamped
self._move_base.send_goal(goal)
class 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'
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 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 DropService():
def __init__(self):
rospy.init_node(NAME + 'server' , anonymous=True)
self.client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
self.client.wait_for_server()
self.gripper_client.wait_for_server()
self.service = rospy.Service('drop_service', ArmDropService, self.handle_drop)
rospy.loginfo("%s: Ready for Dropping", NAME)
#drop it like it's hot
def handle_drop(self, req):
rospy.loginfo("told to drop")
success = False
#this seems to be as far as it can open (both straight ahead), same vals anh uses
if self.move_gripper(0.2, -0.2):
success = True
return success
#stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks
def move_gripper(self, left_finger, right_finger):
goal = SmartArmGripperGoal()
goal.target_joints = [left_finger, right_finger]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_goal_to_finish()
result = self.gripper_client.get_result()
if result.success == False:
rospy.loginfo("Drop failed")
else:
rospy.loginfo("Object Released! Hopefully it isn't sticky")
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 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 MotionPlayer(object):
def __init__(self):
rospy.loginfo("Initializing MotionPlayer...")
self.ac = SimpleActionClient('/play_motion', PlayMotionAction)
rospy.loginfo("Connecting to /play_motion...")
# If more than 5s pass by, we crash
if not self.ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr("Could not connect to /tts action server!")
exit(0)
rospy.loginfo("Connected!")
def play_motion(self, motion_name, block=True):
# To check motions:
# rosparam list | grep play_motion | grep joints
# for example:
# open_hand, close_hand, offer_hand, wave
# shake_hands, thumb_up_hand, pointing_hand, pinch_hand
# head_tour, do_weights, pick_from_floor
g = PlayMotionGoal()
g.motion_name = motion_name
if block:
self.ac.send_goal_and_wait(g)
else:
self.ac.send_goal(g)
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_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 Hello:
"""
a class to greet people need to recognise people
"""
def __init__(self):
self.voice = rospy.get_param("~voice", "voice_en1_mbrola")
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(1)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
#set action server
self.client = SimpleActionClient('face_recognition', face_recognition.msg.FaceRecognitionAction)
# listening for goals.
self.client.wait_for_server()
rospy.sleep(2)
#result will be published on this topic
rospy.Subscriber("/face_recognition/result", FaceRecognitionActionFeedback, self.face_found)
rospy.Subscriber("/face_recognition/feedback", FaceRecognitionActionFeedback, self.Unknown)
self.look_for_face()
def look_for_face(self):
'''
send command look for face once
'''
goal = face_recognition.msg.FaceRecognitionGoal(order_id=0, order_argument="none")
self.client.send_goal(goal)
#self.client.wait_for_result(rospy.Duration.from_sec(6.0))
#if self.client.get_state() == GoalStatus.FAILED:
#self.unknown()
def face_found(self,msg):
'''
clean up feedback to extract the name
check if greeted before answer accordingly
'''
person = str(msg.result.names[0])
rospy.logwarn(person)
self.soundhandle.say("hello "+ person ,self.voice)
#self.task1 = "hello" + person
#rospy.sleep(2)
#return the processed value
self.value = str(self.task1)
def Unknown(self,msg):
self.soundhandle.say("hello what is your name " ,self.voice)
def __str__(self):
return
class Arm:
def __init__(self, arm):
if arm == 'r':
self.arm_client = SAC('r_arm_controller/joint_trajectory_action', JointTrajectoryAction)
self.arm = arm
elif arm == 'l':
self.arm_client = SAC('l_arm_controller/joint_trajectory_action', JointTrajectoryAction)
self.arm = arm
self.arm_client.wait_for_server()
rospy.loginfo('Waiting for server...')
def move(self, angles, time, blocking):
angles = [angles]
times = [time]
timeout=times[-1] + 1.0
goal = JointTrajectoryGoal()
goal.trajectory.joint_names =self.get_joint_names(self.arm)
for (ang, t) in zip(angles, times):
point = JointTrajectoryPoint()
point.positions = ang
point.time_from_start = rospy.Duration(t)
goal.trajectory.points.append(point)
goal.trajectory.header.stamp = rospy.Time.now()
self.arm_client.send_goal(goal)
rospy.sleep(.1)
rospy.loginfo("command sent to client")
status = 0
if blocking: #XXX why isn't this perfect?
end_time = rospy.Time.now() + rospy.Duration(timeout+ .1)
while (
(not rospy.is_shutdown()) and\
(rospy.Time.now() < end_time) and\
(status < gs.SUCCEEDED) and\
(type(self.arm_client.action_client.last_status_msg) != type(None))):
status = self.arm_client.action_client.last_status_msg.status_list[-1].status #XXX get to 80
rospy.Rate(10).sleep()
if status >gs.SUCCEEDED:
rospy.loginfo("goal status achieved. exiting")
else:
rospy.loginfo("ending due to timeout")
result = self.arm_client.get_result()
return result
def get_joint_names(self, char):
return [char+'_shoulder_pan_joint',
char+'_shoulder_lift_joint',
char+'_upper_arm_roll_joint',
char+'_elbow_flex_joint',
char+'_forearm_roll_joint',
char+'_wrist_flex_joint',
char+'_wrist_roll_joint' ]
class Kill(HandlerBase):
alarm_name = 'kill'
def __init__(self):
self._killed = False
self.initial_alarm = Alarm(self.alarm_name, True,
node_name='alarm_server',
problem_description='Initial kill')
self.bag_client = SimpleActionClient("/online_bagger/bag", BagOnlineAction)
self.task_client = TaskClient()
self.first = True
def _online_bagger_cb(self, status, result):
if status == 3:
rospy.loginfo('KILL BAG WRITTEN TO {}'.format(result.filename))
else:
rospy.logwarn('KILL BAG {}, status: {}'.format(TerminalState.to_string(status), result.status))
def _kill_task_cb(self, status, result):
if status == 3:
rospy.loginfo('Killed task success!')
return
rospy.logwarn('Killed task failed ({}): {}'.format(TerminalState.to_string(status), result.result))
def raised(self, alarm):
self._killed = True
if self.first:
self.first = False
return
if 'BAG_ALWAYS' not in os.environ or 'bag_kill' not in os.environ:
rospy.logwarn('BAG_ALWAYS or BAG_KILL not set. Not making kill bag.')
else:
goal = BagOnlineGoal(bag_name='kill.bag')
goal.topics = os.environ['BAG_ALWAYS'] + ' ' + os.environ['bag_kill']
self.bag_client.send_goal(goal, done_cb=self._online_bagger_cb)
self.task_client.run_task('Killed', done_cb=self._kill_task_cb)
def cleared(self, alarm):
self._killed = False
def meta_predicate(self, meta_alarm, alarms):
ignore = []
# Don't kill on low battery, only on critical
# if alarms['battery-voltage'].raised and alarms['battery-voltage'].severity < 2:
# ignore.append('battery-voltage')
# Raised if any alarms besides the two above are raised
raised = [name for name, alarm in alarms.items() if name not in ignore and alarm.raised]
if len(raised):
return Alarm('kill', True, node_name=rospy.get_name(),
problem_description='Killed by meta alarm(s) ' + ', '.join(raised),
parameters={'Raised': raised})
return self._killed
class RecordPoseStampedFromPlayMotion():
"""Manage the learning from a play motion gesture"""
def __init__(self):
rospy.loginfo("Initializing RecordFromPlayMotion")
rospy.loginfo("Connecting to AS: '" + PLAY_MOTION_AS + "'")
self.play_motion_as = SimpleActionClient(PLAY_MOTION_AS, PlayMotionAction)
self.play_motion_as.wait_for_server()
rospy.loginfo("Connected.")
self.current_rosbag_name = "uninitialized_rosbag_name"
self.lfee = LearnFromEndEffector(['/tf_to_ps'], ['right_arm'])
def play_and_record(self, motion_name, groups=['right_arm'], bag_name="no_bag_name_set"):
"""Play the specified motion and start recording poses.
Try to get the joints to record from the metadata of the play_motion gesture
or, optionally, specify the joints to track"""
# Check if motion exists in param server
PLAYMOTIONPATH = '/play_motion/motions/'
if not rospy.has_param(PLAYMOTIONPATH + motion_name):
rospy.logerr("Motion named: " + motion_name + " does not exist in param server at " + PLAYMOTIONPATH + motion_name)
return
else:
rospy.loginfo("Found motion " + motion_name + " in param server at " + PLAYMOTIONPATH + motion_name)
# Get it's info
motion_info = rospy.get_param(PLAYMOTIONPATH + motion_name)
# check if joints was specified, if not, get the joints to actually save
if len(groups) > 0:
joints_to_record = groups
else:
joints_to_record = motion_info['groups']
rospy.loginfo("Got groups: " + str(joints_to_record))
# play motion
rospy.loginfo("Playing motion!")
pm_goal = PlayMotionGoal(motion_name, False, 0)
self.play_motion_as.send_goal(pm_goal)
self.lfee.start_learn(motion_name, bag_name)
done_with_motion = False
while not done_with_motion:
state = self.play_motion_as.get_state()
#rospy.loginfo("State is: " + str(state) + " which is: " + goal_status_dict[state])
if state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED:
done_with_motion = True
elif state != GoalStatus.PENDING and state != GoalStatus.ACTIVE:
rospy.logerr("We got state " + str(state) + " unexpectedly, motion failed. Aborting.")
return None
rospy.sleep(0.1)
# data is written to rosbag in here
motion_data = self.lfee.stop_learn()
return motion_data
def 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 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 HeadObjectTracking():
def __init__(self):
name_space = '/head_traj_controller/point_head_action'
self.head_client = SimpleActionClient(name_space, PointHeadAction)
self.head_client.wait_for_server()
self.curr_tracking_point = Point(1, 0, 0.5)
self.point_head(self.curr_tracking_point.x, self.curr_tracking_point.y, self.curr_tracking_point.z)
rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.new_tracking_data)
def new_tracking_data(self, marker):
"""
Adds a new tracking data point for the head.
Points the head to a point taken as a moving average over some number of
previous tracking data points.
"""
pos = marker.pose.pose.position
OLD_DATA_WEIGHT = .3
# calculate the moving average of the x, y, z positions
tracking_point = self.curr_tracking_point
avg_x = (self.curr_tracking_point.x * OLD_DATA_WEIGHT) + (pos.x * (1 - OLD_DATA_WEIGHT))
avg_y = (self.curr_tracking_point.y * OLD_DATA_WEIGHT) + (pos.y * (1 - OLD_DATA_WEIGHT))
avg_z = (self.curr_tracking_point.z * OLD_DATA_WEIGHT) + (pos.z * (1 - OLD_DATA_WEIGHT))
# make a new averaged point to track and point the head there
self.curr_tracking_point = Point(avg_x, avg_y, avg_z)
self.point_head(avg_x, avg_y, avg_z)
def point_head(self, x, y, z):
"""
Point the head to the specified point
"""
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = '/torso_lift_link'
head_goal.max_velocity = .3
# The transform seems to aim high. Move it down a little...
head_goal.target.point = Point(x, y, z - .4)
rospy.logdebug('Moving head to\n' + str(head_goal.target.point))
self.head_client.send_goal(head_goal)
class Kill(HandlerBase):
alarm_name = 'kill'
def __init__(self):
self._killed = False
self.initial_alarm = Alarm(self.alarm_name, True,
node_name='alarm_server',
problem_description='Initial kill')
self.bag_client = SimpleActionClient("/online_bagger/bag", BagOnlineAction)
self.first = True
def _online_bagger_cb(self, status, result):
if status == 3:
rospy.loginfo('KILL BAG WRITTEN TO {}'.format(result.filename))
else:
rospy.logwarn('KILL BAG {}, status: {}'.format(TerminalState.to_string(status), result.status))
def raised(self, alarm):
self._killed = True
if self.first:
self.first = False
return
if 'BAG_ALWAYS' not in os.environ or 'bag_kill' not in os.environ:
rospy.logwarn('BAG_ALWAYS or BAG_KILL not set. Not making kill bag.')
return
goal = BagOnlineGoal(bag_name='kill.bag')
goal.topics = os.environ['BAG_ALWAYS'] + ' ' + os.environ['bag_kill']
self.bag_client.send_goal(goal, done_cb=self._online_bagger_cb)
def cleared(self, alarm):
self._killed = False
def meta_predicate(self, meta_alarm, alarms):
if self._killed: # Stay killed until manually cleared
return True
ignore = []
# Don't kill on low battery, only on critical
# if alarms['battery-voltage'].raised and alarms['battery-voltage'].severity < 2:
# ignore.append('battery-voltage')
# Raised if any alarms besides the two above are raised
return any([alarm.raised for name, alarm in alarms.items()
if name not in ignore])
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 fixHeadPosition(userdata):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "base_link"
head_goal.target.point.x = 1.0
head_goal.target.point.y = 0.0
head_goal.target.point.z = 1.65
head_goal.pointing_frame = "stereo_link"
head_goal.pointing_axis.x = 1.0
head_goal.pointing_axis.y = 0.0
head_goal.pointing_axis.z = 0.0
head_goal.max_velocity = 1.5
head_goal.min_duration.secs = 1.5
client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction)
client.wait_for_server(rospy.Duration(5.0))
client.send_goal(head_goal)
return succeeded
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 tilt_head(self, down=True):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = self.get_frame()
head_goal.min_duration = rospy.Duration(0.3)
if down:
head_goal.target.point = Point(1, 0, Head.speed * -0.1)
else:
head_goal.target.point = Point(1, 0, Head.speed * 0.1)
head_goal.max_velocity = 10.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(2.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def gripper_action(self, gripper_type, gripper_position):
name_space = '/' + gripper_type + '_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = gripper_position
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
# update the state of the current gripper being modified
if (gripper_type == 'l'):
if (gripper_position == 0.0):
self.left_gripper_state = 'closed'
else:
self.left_gripper_state = 'open'
else:
if (gripper_position == 0.0):
self.right_gripper_state = 'closed'
else:
self.right_gripper_state = 'open'
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 run(self):
# pub = rospy.Publisher('joy_cmd', JoyDriveStamped, queue_size=1)
# r = rospy.Rate(10)
simpleac = SimpleActionClient("move_base", MoveBaseAction)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = '/map'
while not rospy.is_shutdown():
buf = self.conn.recvfrom(struct.calcsize(self.ctrl_def))
self.data = struct.unpack(self.ctrl_def, buf[0])
goal.target_pose.header.stamp = rospy.Time.from_sec(self.data[0])
goal.target_pose.pose.position.x = self.data[1]
goal.target_pose.pose.position.y = self.data[2]
goal.target_pose.pose.orientation.z = self.data[3]
goal.target_pose.pose.orientation.w = self.data[4]
print self.data
simpleac.send_goal(goal)
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')
