Mon 10. Nov 2014, 10:40
Mon 10. Nov 2014, 10:40
Mon 10. Nov 2014, 11:24
Mon 10. Nov 2014, 13:36
Mon 10. Nov 2014, 13:46
Mon 10. Nov 2014, 14:01
Mon 10. Nov 2014, 14:47
Mon 10. Nov 2014, 15:19
#!/usr/bin/env python
'''
Created on Jun 23, 2014
@author: Bernd Winkler
'''
import os
import shutil
from subprocess32 import Popen
import datetime
import traceback
import numpy as np
import math
import rospy
import tf
import actionlib
import std_msgs.msg
import geometry_msgs.msg
import std_srvs.srv
# for robot control
# import ipa325_msgs.msg
import control_msgs.msg
import trajectory_msgs.msg
from ipa325_com_msgs.srv import * # SolveTask, InitTask, ExecuteTask, ExecuteTaskResponse
from ipa325_com_msgs.msg import SolveTaskAction, SolveTaskGoal
current_dir = os.path.dirname(__file__)
gazebo_human_move_path = os.path.abspath(current_dir + '/../../ipa325_euroc_human_move/scripts')
os.sys.path.append(gazebo_human_move_path)
from gazebo_human_move import gazebo_human_move as human_mover
from gazebo_msgs.srv import GetModelState, SetModelState, SetModelStateRequest, GetLinkState, SetLinkState
# import ipa325_gazebo_plugins.msg
import sensor_msgs.msg
import glob
import time
import atexit
import threading
import random
import subprocess32
param_bagfile_base_path = '~/euroc_track1_logging/'
param_run_as_test = None
def gazebo_get_modelstate(model_name, relative_entity_name):
service_name = '/gazebo/get_model_state'
rospy.wait_for_service(service_name)
try:
service = rospy.ServiceProxy(service_name, GetModelState)
resp = service(model_name, relative_entity_name)
return resp
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def gazebo_set_modelstate(model_name, relative_entity_name, ms):
service_name = '/gazebo/set_model_state'
rospy.wait_for_service(service_name)
try:
service = rospy.ServiceProxy(service_name, SetModelState)
resp = service(ms)
return resp
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def gazebo_get_linkstate(name, reference_name):
service_name = '/gazebo/get_link_state'
service_type = GetLinkState
rospy.wait_for_service(service_name)
try:
service = rospy.ServiceProxy(service_name, service_type)
resp = service(name, reference_name)
return resp
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def gazebo_set_linkstate(name, reference_name, linkstate):
service_name = '/gazebo/set_link_state'
service_type = SetLinkState
rospy.wait_for_service(service_name)
try:
service = rospy.ServiceProxy(service_name, service_type)
resp = service(linkstate)
return resp
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
simulation_environment = None
bag_logger = None
class com_server(object):
active_task = ""
active_task_topic = None
task_ids = ['1.1', '1.2', '1.3', '2']
task_time_budget_s = {'1.1':0, '1.2':30, '1.3':30, '2':600}
t2_param_insertion_depth = 0.006 # tcp has to be 6mm into workpiece surface to
general_log_topics = ['tf', 'rosout', 'active_task', 'time_budget']
tf_frames_lock = threading.Lock()
do_tf_broadcast_thread = None
tf_frames_dict = {"/hole_guess":([0.0, -0.61, 0.5], [0.3, 0.3, 0.3, 0]),
"/rivet_home":([0.0, -0.61, 0.5], [0.3, 0.3, 0.3, 0])}
tf_listener = None
client_ur_joint = None
# complete list of hole names
hole_tf_framenames = ['h0', 'h1', 'h2', 'h3', 'h4', 'h5', 'h6', 'h7', 'h8', 'h9', 'h10', 'h11', 'h12', 'h13', 'h14', 'h15', 'h16', 'h17', 'h18', 'h19', 'h20', 'h21', 'h22', 'h23', 'h24', 'h25', 'h26', 'h27', 'h28', 'h29', 'h30', 'h31', 'h32', 'h33']
t1_1_trajectory_count = 20
timestep = 1 # time countdown in 10 s steps
t2_sm_lock = threading.Lock()
t2_sm_state = ""
t2_cur_hole_tf_framename = None
t2_run_human_interference = None
t2_topic_target_frame = None
t0_pub_timebudget = None
tf_broadcaster_pointer_lock = threading.Lock()
tf_broadcaster_pointer = tf.TransformBroadcaster()
def init_for_test(self):
rospy.loginfo("initializing for test")
self.task_time_budget_s = {'1.1':0, '1.2':10, '1.3':30 , '2':200}
self.task_ids = ['1.1', '1.2', '1.3', '2']
self.t1_1_trajectory_count = 3
# dont move to any real hole but to hole_guess
self.hole_tf_framenames = ['hole_guess']
pass
def home_robot(self):
# Todo: test if robot is already there
rospy.loginfo("moving robot to home position")
goal = control_msgs.msg.FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]
p = trajectory_msgs.msg.JointTrajectoryPoint()
p.positions = [0.1, -math.pi / 2 + 0.1, 0.1, -math.pi / 2 + 0.1, 0.1, 0.1]
p.time_from_start = rospy.Duration(1)
goal.trajectory.points.append(p)
# Send FoJoTra goal
# rospy.loginfo("sending goal to robot")
self.client_ur_joint.send_goal(goal)
self.client_ur_joint.wait_for_result()
rospy.loginfo("robot in home position")
def do_tf_broadcast(self):
try:
tf_broadcaster = tf.TransformBroadcaster()
rate = rospy.Rate(50.0)
while not rospy.is_shutdown():
with self.tf_frames_lock:
for frame_name in self.tf_frames_dict:
# publish tf frames
# rospy.loginfo("publishing " + frame_name)
tf_broadcaster.sendTransform(*self.tf_frames_dict[frame_name], time=rospy.Time.now(), child=frame_name, parent="/ur_base")
rate.sleep()
except:
traceback.print_exc()
pass
rospy.loginfo("leaving thread do_tf_broadcast")
def cb_range_to_tf_frame(self, msg):
ranges = msg.ranges
dist = ranges[2]
with self.tf_broadcaster_pointer_lock:
self.tf_broadcaster_pointer.sendTransform(translation=(dist, 0, 0), rotation=(1, 0, 0, 0), time=rospy.Time.now(), child="pointed_to", parent="/human_arm_r_pointer")
def __init__(self):
atexit.register(self._atexit)
# use special setup for testing
if param_run_as_test:
self.init_for_test()
# paths used
directory = os.path.expanduser(param_bagfile_base_path)
# make sure it exists
if not os.path.exists(directory):
os.makedirs(directory)
if not os.path.exists(directory):
rospy.logerr("unable to create directory for logging " + directory)
self.logging_dir = directory
rospy.loginfo("log files will be written to " + directory)
self.tf_listener = tf.TransformListener()
self.sub_pointer = rospy.Subscriber("pointer", sensor_msgs.msg.LaserScan , self.cb_range_to_tf_frame)
self.sub_collision_human = rospy.Subscriber("collision_human_robot", std_msgs.msg.Bool , self.cb_on_human_robotmoving_collision)
self.sub_collision_workpiece = rospy.Subscriber("/gazebo/collisions/ur5_workpiece", std_msgs.msg.Bool , self.cb_on_robot_workpiece_collision)
# register converter (tf->topic) for workpiece pose
self.t1_1_sub_wp_pose = rospy.Subscriber("t1_2_workpiece", geometry_msgs.msg.PoseStamped, self.t1_2_cb_workpiece_pose_converter)
self.t1_1_pub_wp_pose_gt = rospy.Publisher("t1_2_workpiece_gt", geometry_msgs.msg.PoseStamped)
self.t1_3_sub_pointer_pose = rospy.Subscriber("t1_3_pointedpose", geometry_msgs.msg.PoseStamped, self.t1_3_cb_pointed_pose_converter)
self.t1_3_pub_pointer_pose_gt = rospy.Publisher("t1_3_pointedpose_gt", geometry_msgs.msg.PoseStamped)
self.t2_pub_hole_pose_gt = rospy.Publisher("t2_holepose_gt", geometry_msgs.msg.PoseStamped)
self.t2_pub_tcp_pose = rospy.Publisher("t2_tcppose", geometry_msgs.msg.PoseStamped)
rospy.loginfo("waiting for simulator")
s = rospy.wait_for_message("simulation_running", std_msgs.msg.String)
if s == None:
rospy.logerr("SIMULATOR HAS TO BE STARTED EXTERNALLY!!! DOESNT APPEAR SO RIGHT NOW, BUT LETS SEE :-)")
# get control of the scene
self.client_ur_joint = actionlib.SimpleActionClient('/simulation_ur5/joint_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
if not self.client_ur_joint.wait_for_server():
rospy.logerr("client_ur_joint: actionclient not found")
raise Exception()
self.reset_simulator()
time.sleep(4) # let settle
# backup initial human position
scoped_link_name = "human"
relative_entity_name = "world"
# get current workpiece pose
resp = gazebo_get_modelstate(scoped_link_name, relative_entity_name)
ms_req = SetModelStateRequest()
# Set Gazebo Model pose and twist
# string model_name # model to set state (pose and twist)
# geometry_msgs/Pose pose # desired pose in reference frame
# geometry_msgs/Twist twist # desired twist in reference frame
# string reference_frame # set pose/twist relative to the frame of this entity (Body/Model)
# # leave empty or "world" or "map" defaults to world-frame
ms_req.model_state.model_name = scoped_link_name
ms_req.model_state.reference_frame = relative_entity_name
ms_req.model_state.pose = resp.pose
ms_req.model_state.twist = resp.twist
self.gz_modelstate_human_init = ms_req.model_state
# backup workpiece initial pose
self.gz_workpiecepose_initial = self.get_workpiece_pose()
self.do_tf_broadcast_thread = threading.Thread(target=self.do_tf_broadcast)
self.do_tf_broadcast_thread.start()
# start topics
self.active_task_topic = rospy.Publisher('active_task', std_msgs.msg.String, latch=True)
self.active_task_topic.publish("0")
self.t0_pub_timebudget = rospy.Publisher('time_budget', std_msgs.msg.Int32, latch=True)
self.t0_pub_timebudget.publish(0)
# start action client
self.client = actionlib.SimpleActionClient('solve_task', SolveTaskAction)
rospy.loginfo("waiting for client")
self.client.wait_for_server()
# start services
self.t_2_service_load_rivet = rospy.Service("load_rivet", std_srvs.srv.Empty, self.cb_load_rivet)
self.t_2_service_trigger_rivet = rospy.Service("trigger_rivet", std_srvs.srv.Empty, self.cb_trigger_rivet)
self.service_execute_task = rospy.Service("execute_task", ExecuteTask, self.cb_execute_task)
rospy.loginfo("service 'execute_task' ready for requests")
def _atexit(self):
self.stop_logging()
try:
self.do_tf_broadcast_thread.join(10)
except:
rospy.logerr("couldn't join tf broadcast thread")
def _cancel_goal_wait(self):
'''cancels last goal and waits until acknowledged'''
self.client.cancel_goal()
self.client.wait_for_result(timeout=rospy.Duration(10))
def get_trajectory_fns(self, traj_dir_pattern="/trajectories_p?/trajectory_*.yaml"):
module_path = __file__ # path of current module
dirname_cur = os.path.dirname(module_path)
dirname_traj = dirname_cur + "/../../ipa325_euroc_human_move/yaml"
dirname_traj = os.path.normpath(dirname_traj)
glob_pattern = dirname_traj + traj_dir_pattern
traj_pathfns = glob.glob(glob_pattern)
if len(traj_pathfns) == 0:
raise Exception("on trajectories found using pattern " + glob_pattern)
rospy.loginfo("used pattern for traj glob: %s" % glob_pattern)
return traj_pathfns
def init_task_caller(self, taskid):
rospy.loginfo("asking client to prepare for task" + taskid)
found = rospy.wait_for_service('/init_task')
init_task_service = rospy.ServiceProxy('/init_task', InitTask)
init_task_service(taskid)
rospy.loginfo('preparing for task ' + taskid + ' done')
def get_workpiece_pose(self):
r"""
return current workpiece pose as link state
"""
scoped_link_name = "workpiece::workpiece_link"
relative_entity_name = "world"
# get current workpiece pose
ls_old = gazebo_get_linkstate(scoped_link_name, relative_entity_name)
if not ls_old.success:
raise Exception("unable to retrieve link state")
return ls_old.link_state
def set_workpiece_pose(self, linkstate):
scoped_link_name = "workpiece::workpiece_link"
relative_entity_name = "world"
# set workpiece pose
gazebo_set_linkstate(scoped_link_name, relative_entity_name, linkstate)
def change_workpiece_pose(self):
r"""
change workpiece pose with small random noise
"""
# get current workpiece pose
ls_new = self.get_workpiece_pose()
# add noise
ls_new.pose.position.x += random.uniform(-0.03, 0.03)
ls_new.pose.position.y += random.uniform(-0.03, 0.03)
ls_new.pose.position.z += 0.0001
q = ls_new.pose.orientation
euler_orig = list(tf.transformations.euler_from_quaternion((q.x, q.y, q.z, q.w)))
euler_orig[2] += random.uniform(-0.2, 0.2)
q_new = tf.transformations.quaternion_from_euler(*euler_orig)
(ls_new.pose.orientation.x, ls_new.pose.orientation.y, ls_new.pose.orientation.z, ls_new.pose.orientation.w) = (q_new[0], q_new[1], q_new[2], q_new[3])
# set pose
self.set_workpiece_pose(ls_new)
rospy.loginfo("changed pose of workpiece")
def reset_simulator(self):
# ## resetting gazebo world
reset_call = rospy.ServiceProxy("/gazebo/reset_world", std_srvs.srv.Empty)
reset_call()
rospy.loginfo("Resetting Gazebo")
rospy.sleep(5)
self.home_robot()
def start_logging(self, topics=[]):
global bag_logger
# determine filename to which to log
timestamp = datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%S")
if param_run_as_test:
self.bag_fn = "{}_task_{}.bag".format(timestamp, self.active_task)
else:
self.bag_fn = "{}_task_{}.bag".format(timestamp, self.active_task)
# determine relevant topics to log
relevant_topics = topics + self.general_log_topics
args = ['rosbag', 'record', '-O' + self.bag_fn] + relevant_topics + ['__name:=ipa325_record']
rospy.loginfo("start logging of the following topics: %s" % str(relevant_topics))
bag_logger = Popen(args)
# wait for logging really in process
bag_current_filepath = os.path.join(os.getcwd(), self.bag_fn)
while not os.path.isfile(bag_current_filepath + ".active"):
time.sleep(1)
rospy.loginfo("logging started")
pass
def stop_logging(self):
global bag_logger
if bag_logger is not None:
try:
subprocess32.call(['rosnode', 'kill', 'ipa325_record'])
rospy.loginfo("logging stopped")
except:
rospy.loginfo("Unexpected error:" + sys.exc_info())
rospy.loginfo("except")
pass
bag_logger = None
bag_current_filepath = os.path.join(os.getcwd(), self.bag_fn)
if not os.path.isfile(bag_current_filepath + ".active"):
rospy.logwarn("no bagfile created")
return
bag_target_filepath = os.path.join(self.logging_dir, self.bag_fn)
cnt = 0
max_cnt = 10
while (not os.path.isfile(bag_current_filepath) and cnt < max_cnt):
rospy.loginfo('Waiting for bagfile to be ready')
# print 'Counter:', cnt
cnt = cnt + 1
rospy.sleep(1)
if (cnt >= max_cnt):
rospy.logerr('bagfile not ready at' + bag_current_filepath)
else:
rospy.loginfo('file found')
shutil.move(bag_current_filepath, bag_target_filepath)
def cleanup(self, taskid):
# cancel the solve_task action call (don't wait for the result)
self._cancel_goal_wait()
rospy.loginfo('execution of task finished: ' + taskid)
self.active_task = ""
# stop logger
self.stop_logging()
self.active_task_topic.publish("0")
def check_topics(self, topics_dict):
'''test if the given topics are available'''
for topicname_expected in topics_dict.keys():
m = rospy.wait_for_message(topicname_expected, topics_dict[topicname_expected], timeout=10)
if m == None:
rospy.logerr("didn't find topic {} of type {} that has to be published by team".format(topicname_expected, topics_dict[topicname_expected]))
raise Exception("topic missing")
tf_lock = threading.Lock()
def getcurrentposec(self, frame_from='/world', frame_to='/tcp'):
"""delivers current cartesian position (trans,rot_tf)"""
with self.tf_lock:
while not rospy.is_shutdown():
try:
return self.tf_listener.lookupTransform(frame_from, frame_to, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logdebug("waiting for tf: " + frame_from + ' -> ' + frame_to)
time.sleep(0.05)
continue
def get_next_hole_frame_name(self):
self.t2_cur_hole_tf_framename = random.choice(self.hole_tf_framenames)
rospy.loginfo("next hole's frame is " + self.t2_cur_hole_tf_framename)
return self.t2_cur_hole_tf_framename
def t2_statemachine(self, targetstate):
# print targetstate
if self.active_task != '2':
return
try:
with self.t2_sm_lock:
if targetstate == 'home':
if self.t2_sm_state == 'hole':
# test if hole was reached
# if rivet is sufficiently deep inserted (test in z direction)
(trans, rot_tf) = self.getcurrentposec(self.t2_cur_hole_tf_framename, '/tcp')
in_hole = trans[2] < self.t2_param_insertion_depth and np.linalg.norm(trans[:2]) < 0.003 # inserted 6mm deep, xy distance of 3 mm
# assume we get sometimes close enough
if param_run_as_test:
in_hole = random.choice([True, False])
if not in_hole:
rospy.logwarn("rivet is not sufficiently deep inserted. please adjust.")
return
# publish hole pose and tcp pose as topic
rospy.logdebug("hole pose gt")
if param_run_as_test:
gt_pos, gt_rottf = self.getcurrentposec("/workpiece_link", "hole_guess") # try to reach hole_guess
else:
gt_pos, gt_rottf = self.getcurrentposec("/workpiece_link", self.current_hole_tf_frame_name) # try to reach real hole
ps = geometry_msgs.msg.PoseStamped()
ps.pose.position.x = gt_pos[0]
ps.pose.position.y = gt_pos[1]
ps.pose.position.z = gt_pos[2]
ps.pose.orientation.x = gt_rottf[0]
ps.pose.orientation.y = gt_rottf[1]
ps.pose.orientation.z = gt_rottf[2]
ps.pose.orientation.w = gt_rottf[3]
ps.header.frame_id = "workpiece_link"
self.t2_pub_hole_pose_gt.publish(ps)
rospy.sleep(0.5)
rospy.logdebug("tcp pose after riveting")
gt_pos, gt_rottf = self.getcurrentposec("/workpiece_link", "/tcp")
ps = geometry_msgs.msg.PoseStamped()
ps.pose.position.x = gt_pos[0]
ps.pose.position.y = gt_pos[1]
ps.pose.position.z = gt_pos[2]
ps.pose.orientation.x = gt_rottf[0]
ps.pose.orientation.y = gt_rottf[1]
ps.pose.orientation.z = gt_rottf[2]
ps.pose.orientation.w = gt_rottf[3]
ps.header.frame_id = "workpiece_link"
self.t2_pub_tcp_pose.publish(ps)
# set topic target_frame to 'rivet_home'
self.t2_topic_target_frame.publish('/rivet_home')
rospy.loginfo("reached hole position")
self.t2_sm_state = targetstate # switch to next state
pass
elif targetstate == 'hole':
if self.t2_sm_state == 'home':
# test if home was reached
(trans, rot_tf) = self.getcurrentposec('/rivet_home', '/tcp')
at_home = np.linalg.norm(trans) < 0.01 # 10 mm tolerance
if not at_home:
rospy.logwarn("not yet reached home position. please adjust.")
return
# robot reached home: test if collision occured before
if self.t2_workpiece_collision:
# reset workpiece position
self.set_workpiece_pose(self.gz_workpiecepose_initial)
# plus noise
self.change_workpiece_pose()
self.t2_workpiece_collision = False
time.sleep(5)
# move tf frame hole_guess to hole + limited random offset
self.current_hole_tf_frame_name = self.get_next_hole_frame_name()
(gt_trans, gt_rot_tf) = self.getcurrentposec('/ur_base', self.current_hole_tf_frame_name)
new_trans = np.asarray(gt_trans) + np.random.uniform(-0.003, 0.003, 3) # +-3m mm offset possible
new_rot_tf = np.asarray(gt_rot_tf) * random.uniform(0, 0.1) # some rotational noise as well
with self.tf_frames_lock:
self.tf_frames_dict['/hole_guess'] = (list(new_trans), list(new_rot_tf))
# set topic target_frame to 'hole_guess'
self.t2_topic_target_frame.publish('/hole_guess')
rospy.loginfo("reached home position")
self.t2_sm_state = targetstate
pass
elif targetstate == 'collision_workpiece':
rospy.logerr("workpiece/table-robot collision detected")
# set topic target_frame to 'rivet_home'
self.t2_workpiece_collision = True
# start over from home
self.t2_topic_target_frame.publish('/rivet_home')
self.t2_sm_state = 'home' # switch to next state
pass
elif targetstate == 'collision_human':
rospy.loginfo("harmfull human-robot collision detected")
# move human out of the way
scoped_link_name = "human::human_base_link"
relative_entity_name = "world"
# TODO switch off human
# reset human to initial pose
gazebo_set_modelstate(scoped_link_name, relative_entity_name, self.gz_modelstate_human_init)
time.sleep(3)
# set topic target_frame to 'rivet_home'
self.t2_topic_target_frame.publish('/rivet_home')
self.t2_sm_state = 'home' # switch to next state
pass
elif targetstate == 'init':
# set topic target_frame to 'rivet_home'
self.t2_topic_target_frame.publish('/rivet_home')
self.t2_sm_state = 'home' # switch to next state
else:
rospy.logerr("state change not expected {} -> {}".format(self.t2_sm_state, targetstate))
raise Exception()
except:
traceback.print_exc()
def cb_load_rivet(self, req):
rospy.logdebug("service load_rivet called")
resp = std_srvs.srv.EmptyResponse()
if self.active_task != '2':
rospy.logwarn("task 2 is not active. skipping call")
return resp
self.t2_statemachine('hole')
return resp
def cb_trigger_rivet(self, req):
rospy.logdebug("service trigger_rivet called")
resp = std_srvs.srv.EmptyResponse()
if self.active_task != '2':
rospy.logwarn("task 2 is not active. skipping call")
return resp
self.t2_statemachine('home')
return resp
def cb_on_human_robotmoving_collision(self, msg):
'''gets called when robot induced collision between human and robot happens'''
hit = msg.data
if hit:
self.t2_statemachine('collision_human')
pass
def cb_on_robot_workpiece_collision(self, msg):
'''gets called when robot collides with table or workpiece'''
hit = msg.data
if hit:
self.t2_statemachine('collision_workpiece')
pass
def do_t2_human_interference(self):
'''launch human interference at random times'''
try:
fns = self.get_trajectory_fns()
rospy.loginfo("found {} trajectories files".format(str(fns)))
while (self.active_task == '2') and not rospy.is_shutdown() and self.t2_run_human_interference:
time.sleep(random.uniform(5, 30))
human_mover(random.choice(fns), reset_world=False)
except:
traceback.print_exc()
def t1_2_cb_workpiece_pose_converter(self, data):
''' republish tf frame in normal topic as PoseStamped'''
rospy.logdebug("received workpiece pose from team")
# received wp pose from team
# extract wp_gt pose from tf
wp_gt_pos, wp_gt_rottf = self.getcurrentposec("/world", "/workpiece_link")
# publish wp_gt pose
ps = geometry_msgs.msg.PoseStamped()
ps.pose.position.x = wp_gt_pos[0]
ps.pose.position.y = wp_gt_pos[1]
ps.pose.position.z = wp_gt_pos[2]
ps.pose.orientation.x = wp_gt_rottf[0]
ps.pose.orientation.y = wp_gt_rottf[1]
ps.pose.orientation.z = wp_gt_rottf[2]
ps.pose.orientation.w = wp_gt_rottf[3]
ps.header.frame_id = "world"
self.t1_1_pub_wp_pose_gt.publish(ps)
def t1_3_cb_pointed_pose_converter(self, data):
''' republish tf frame in normal topic as PoseStamped'''
rospy.logdebug("received pointed_to pose from team")
# received pointedto pose from team
# extract pointer pose from tf
pt_gt_pos, pt_gt_rottf = self.getcurrentposec("/workpiece_link", "/pointed_to")
# publish wp_gt pose on self.t1_1_pub_workpiece_pose_gt
ps = geometry_msgs.msg.PoseStamped()
ps.pose.position.x = pt_gt_pos[0]
ps.pose.position.y = pt_gt_pos[1]
ps.pose.position.z = pt_gt_pos[2]
ps.pose.orientation.x = pt_gt_rottf[0]
ps.pose.orientation.y = pt_gt_rottf[1]
ps.pose.orientation.z = pt_gt_rottf[2]
ps.pose.orientation.w = pt_gt_rottf[3]
ps.header.frame_id = "workpiece_link"
self.t1_3_pub_pointer_pose_gt.publish(ps)
def execute_task_1_1(self):
taskid = "1.1"
# test if other task is running
if self.active_task != "":
rospy.logerr("cannot execute new task. another task is still running: " + self.active_task)
return
self.active_task = taskid
self.active_task_topic.publish(taskid)
self.reset_simulator()
# start logging of relevant topics
own_topics = {'t1_1_gesturerecog_gt':std_msgs.msg.Bool,
't1_1_trajectory_index':std_msgs.msg.Int32}
team_topics = {'t1_1_gesturerecog':std_msgs.msg.Bool}
self.start_logging(own_topics.keys() + team_topics.keys())
# init simulator side with task specifics
# init topics
self.t1_1_pub_gesture_gt = rospy.Publisher('t1_1_gesturerecog_gt', std_msgs.msg.Bool, latch=True)
self.t1_1_pub_trajindex = rospy.Publisher('t1_1_trajectory_index', std_msgs.msg.Int32, latch=True)
self.t1_1_pub_gesture_gt.publish(False)
rospy.sleep(0.2)
self.t1_1_pub_trajindex.publish(-1)
rospy.sleep(1)
# get trajectories
traj_pathfns0 = self.get_trajectory_fns(traj_dir_pattern="/trajectories_p0/trajectory_*.yaml")
traj_pathfns1 = self.get_trajectory_fns(traj_dir_pattern="/trajectories_p1/trajectory_*.yaml")
rospy.logdebug("found {} human trajectory files".format(len(traj_pathfns0) + len(traj_pathfns1)))
self.init_task_caller(taskid)
self.check_topics(team_topics)
# call solve_task action from client's action server
goal = SolveTaskGoal(taskid)
rospy.loginfo("asking client to solve task" + taskid)
self.client.send_goal(goal)
# pick 10 trajectories from each gt (0/1)
import random
traj_pathfns = random.sample(traj_pathfns0, 10) + random.sample(traj_pathfns1, 10)
# random order
random.shuffle(traj_pathfns)
rospy.loginfo("running the following trajectories: \n{}".format(traj_pathfns))
traj_pathfns = traj_pathfns[:self.t1_1_trajectory_count]
for i, traj_fn in enumerate(traj_pathfns):
# publish ground truth on gt topic
if traj_fn.endswith('_p1.yaml'):
self.t1_1_pub_gesture_gt.publish(True)
else:
self.t1_1_pub_gesture_gt.publish(False)
rospy.sleep(0.2)
# publish current trajectory number on index topic
self.t1_1_pub_trajindex.publish(i)
rospy.sleep(1)
# run trajectory
rospy.loginfo("running human trajectory file '%s'" % traj_fn)
human_mover(traj_fn)
rospy.loginfo("finished running human trajectory file")
# signal end of task
self.t1_1_pub_trajindex.publish(-2)
if not self.client.wait_for_result(rospy.Duration(10)):
rospy.logwarn("time is up. canceling task solving")
self.cleanup(taskid)
def execute_task_1_2(self):
taskid = "1.2"
# test if other task is running
if self.active_task != "":
rospy.logerr("cannot execute new task. another task is still running: " + self.active_task)
return
self.active_task = taskid
self.active_task_topic.publish(taskid)
self.reset_simulator()
# start logging of relevant topics
# gt workpiecepose comes from tf
own_topics = {"t1_2_workpiece_gt": geometry_msgs.msg.PoseStamped}
team_topics = {"t1_2_workpiece": geometry_msgs.msg.PoseStamped}
self.start_logging(own_topics.keys() + team_topics.keys())
self.init_task_caller(taskid)
self.check_topics(team_topics)
# call solve_task action from client's action server
goal = SolveTaskGoal(taskid)
# set workpiece pose randomly
self.change_workpiece_pose()
time_budget = self.task_time_budget_s[taskid]
rospy.loginfo("timebudget: " + str(time_budget))
self.t0_pub_timebudget.publish(time_budget)
time.sleep(1)
rospy.loginfo("asking client to solve task" + taskid)
self.client.send_goal(goal)
# time count down
while time_budget >= 0:
time.sleep(self.timestep)
time_budget -= self.timestep
self.t0_pub_timebudget.publish(time_budget)
if not self.client.wait_for_result(rospy.Duration(10)):
rospy.logwarn("time is up. canceling task solving")
self.cleanup(taskid)
def execute_task_1_3(self):
taskid = "1.3"
# test if other task is running
if self.active_task != "":
rospy.logerr("cannot execute new task. another task is still running: " + self.active_task)
return
self.active_task = taskid
self.active_task_topic.publish(taskid)
# reset scene
self.reset_simulator()
# start logging of relevant topics
# gt pointer comes from tf
own_topics = {"t1_3_pointedpose_gt": geometry_msgs.msg.PoseStamped}
team_topics = {"t1_3_pointedpose": geometry_msgs.msg.PoseStamped}
self.start_logging(own_topics.keys() + team_topics.keys())
self.init_task_caller(taskid)
self.check_topics(team_topics)
traj_pathfns = self.get_trajectory_fns("/trajectories_p1_wait/trajectory_*.yaml")
rospy.logdebug("found {} human trajectory files".format(len(traj_pathfns)))
# call solve_task action from client's action server
goal = SolveTaskGoal(taskid)
time_budget = self.task_time_budget_s[taskid]
rospy.loginfo("timebudget: " + str(time_budget))
self.t0_pub_timebudget.publish(time_budget)
time.sleep(1)
rospy.loginfo("asking client to solve task" + taskid)
self.client.send_goal(goal)
# move human and workpiece to position
random.shuffle(traj_pathfns)
traj_fn = traj_pathfns[0]
# set workpiece pose (xyrz) randomly (current pose + jitter)
self.change_workpiece_pose()
rospy.loginfo("running human trajectory file '%s'" % traj_fn)
human_mover(traj_fn, reset_world=False)
rospy.loginfo("finished running human trajectory file")
while time_budget >= 0:
time.sleep(self.timestep)
time_budget -= self.timestep
self.t0_pub_timebudget.publish(time_budget)
if not self.client.wait_for_result(rospy.Duration(10)):
rospy.logwarn("time is up. cancelling task solving")
self.cleanup(taskid)
def execute_task_2(self):
taskid = "2"
# test if other task is running
if self.active_task != "":
rospy.logerr("cannot execute new task. another task is still running: " + taskid)
return
# has to be before setting self.active_task, else the sporadic collisions would trigger the statemachine, which would access this (then uninitialized) publisher
self.t2_topic_target_frame = rospy.Publisher('target_frame', std_msgs.msg.String, latch=True)
self.t2_topic_target_frame.publish('rivet_home')
self.active_task = taskid
self.active_task_topic.publish(taskid)
self.reset_simulator()
self.t2_workpiece_collision = False
# start logging of relevant topics
# gt pointer comes from tf
own_topics = {'target_frame': std_msgs.msg.String,
'rivet_home': std_msgs.msg.String,
'collision_human_robot': std_msgs.msg.Bool,
'collision_workpiece_robot': std_msgs.msg.Bool.data,
't2_holepose_gt': geometry_msgs.msg.PoseStamped,
't2_tcppose': geometry_msgs.msg.PoseStamped
}
team_topics = {}
self.start_logging(own_topics.keys() + team_topics.keys())
self.init_task_caller(taskid)
self.check_topics(team_topics)
self.t2_statemachine('init')
# call solve_task action from client's action server
goal = SolveTaskGoal(taskid)
# start human interference thread
self.t2_run_human_interference = True
t = threading.Thread(target=self.do_t2_human_interference)
t.start()
time_budget = self.task_time_budget_s[taskid]
rospy.loginfo("timebudget: " + str(time_budget))
self.t0_pub_timebudget.publish(time_budget)
time.sleep(1)
rospy.loginfo("asking client to solve task" + taskid)
self.client.send_goal(goal)
# time count down
while time_budget >= 0:
time.sleep(self.timestep)
time_budget -= self.timestep
self.t0_pub_timebudget.publish(time_budget)
if not self.client.wait_for_result(rospy.Duration(10)):
rospy.logwarn("time is up. cancelling task solving")
# end human interference thread
self.t2_run_human_interference = False
rospy.loginfo("waiting for end of human interference thread")
t.join(60)
rospy.loginfo("waiting for end of human interference thread. time out")
self.cleanup(taskid)
def cb_execute_task(self, req):
taskid = req.taskid
if taskid not in self.task_ids:
rospy.logerr('invalid task ' + taskid)
return ExecuteTaskResponse()
rospy.loginfo('executing task ' + taskid)
if taskid == '1.1':
self.execute_task_1_1()
elif taskid == '1.2':
self.execute_task_1_2()
elif taskid == '1.3':
self.execute_task_1_3()
elif taskid == '2':
self.execute_task_2()
rospy.loginfo('finished executing task ' + taskid)
return ExecuteTaskResponse()
def test_find_trajectories(self):
rospy.loginfo("trajectories for task 1.1: \n" + "\n".join(self.get_trajectory_fns()))
rospy.loginfo("trajectories for task 1.3: \n" + "\n".join(self.get_trajectory_fns("/trajectories_p1_wait/trajectory_*.yaml")))
def test_workpiece_positioning(self):
rospy.loginfo("testing workpiece repositioning")
for i in xrange(10):
self.reset_simulator()
self.change_workpiece_pose()
time.sleep(5)
def test_run_challenge(self):
'''function runs through all tasks and triggers task setup, and initialization and execution of team code'''
rospy.loginfo("testing C1S1T1 runability")
found = rospy.wait_for_service('/execute_task')
execute_task_service = rospy.ServiceProxy('/execute_task', ExecuteTask)
test_task_ids = self.task_ids
rospy.loginfo("testing the following tasks: " + str(test_task_ids))
for taskid in test_task_ids:
execute_task_service(taskid)
rospy.loginfo('done with task tests')
def test_robot():
# connect to robot
# create tf broadcaster
# tf_broadcaster.sendTransform(*self.tf_frames_dict[frame_name], time=rospy.Time.now(), child=frame_name, parent="/ur_base")
pass
def run_tests():
# test_robot()
server = com_server()
server.test_run_challenge()
# server.test_find_trajectories()
# server.test_workpiece_positioning()
pass
if __name__ == "__main__":
global param_run_as_test
# init node
rospy.init_node("com_server_node", log_level=rospy.INFO)
try:
param_run_as_test = rospy.get_param("/euroc_test_com_server")
rospy.loginfo("running com_server in test mode")
except:
param_run_as_test = False
if param_run_as_test:
run_tests()
else:
c_server = com_server()
rospy.spin()
pass
Mon 10. Nov 2014, 16:04
Bei iphpbb3.com bekommen Sie ein kostenloses Forum mit vielen tollen Extras
Forum kostenlos einrichten - Hot Topics - Tags
Beliebteste Themen: Cam, Uni, NES, Software, Mode
Impressum | Datenschutz