class Application:
def __init__(self):
# ATF code
self.atf = ATF()
# native app code
self.pub_freq = 20.0 # Hz
self.br = tf.TransformBroadcaster()
rospy.sleep(1) #wait for tf broadcaster to get active (rospy bug?)
def execute(self):
# small testblock (circle r=0.5, time=3)
self.atf.start("testblock_small")
self.pub_tf_circle("link1", "link2", radius=1, time=3)
self.atf.stop("testblock_small")
# large testblock (circle r=1, time=5
self.atf.start("testblock_large")
self.pub_tf_circle("link1", "link2", radius=2, time=5)
self.atf.stop("testblock_large")
self.atf.shutdown()
def pub_tf_circle(self, parent_frame_id, child1_frame_id, radius=1, time=1):
rate = rospy.Rate(int(self.pub_freq))
for i in range(int(self.pub_freq * time) + 1):
t = i / self.pub_freq / time
self.br.sendTransform(
(-radius * math.cos(2 * math.pi * t) + radius, -radius * math.sin(2 * math.pi * t), 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
child1_frame_id,
parent_frame_id)
rate.sleep()
class Application:
def __init__(self):
self.atf = ATF()
def execute(self):
self.atf.start("testblock_1")
rospy.sleep(1)
self.atf.stop("testblock_1")
self.atf.start("testblock_2")
rospy.sleep(1)
self.atf.stop("testblock_2")
self.atf.shutdown()
class Application:
def __init__(self):
self.atf = ATF()
def execute(self):
# Make sure all hardware is up and running (mainly needed for ensenso)
# did not use the ATF wait for topic since this interferes with testing the interface
rospy.sleep(30)
# ROS Camera test
self.atf.start("get_data")
rospy.sleep(10)
self.atf.stop("get_data")
self.atf.shutdown()
class Application:
def __init__(self):
self.atf = ATF()
def execute(self):
self.atf.start("testblock_8s")
self.atf.start("testblock_3s")
rospy.sleep(3)
self.atf.stop("testblock_3s")
self.atf.start("testblock_5s")
rospy.sleep(5)
self.atf.stop("testblock_5s")
#self.atf.stop("testblock_8s")
self.atf.shutdown()
class Application:
def __init__(self):
self.atf = ATF()
def execute(self):
self.atf.start("testblock_8s")
self.atf.start("testblock_3s")
rospy.sleep(3)
self.atf.stop("testblock_3s")
self.atf.start("testblock_5s")
rospy.sleep(5)
self.atf.stop("testblock_5s")
#self.atf.stop("testblock_8s")
self.atf.shutdown()
class Application:
def __init__(self):
self.atf = ATF()
self.ptf = PublishTf()
def execute(self):
self.atf.start("testblock_all")
# circle
self.atf.start("testblock_circle")
self.ptf.pub_circ(radius=1, time=5)
self.atf.stop("testblock_circle")
# quadrat
self.atf.start("testblock_quadrat")
self.ptf.pub_quadrat(length=2, time=10)
self.atf.stop("testblock_quadrat")
self.atf.stop("testblock_all")
self.atf.shutdown()
class Application:
def __init__(self):
# ATF code
self.atf = ATF()
# native app code
self.pub_freq = 20.0 # Hz
self.br = tf.TransformBroadcaster()
rospy.sleep(1) #wait for tf broadcaster to get active (rospy bug?)
def execute(self):
# small testblock (circle r=0.5, time=3)
self.atf.start("testblock_small")
self.pub_tf_circle("link1", "link2", radius=1, time=3)
self.atf.stop("testblock_small")
# large testblock (circle r=1, time=5
self.atf.start("testblock_large")
self.pub_tf_circle("link1", "link2", radius=2, time=5)
self.atf.stop("testblock_large")
self.atf.shutdown()
def pub_tf_circle(self,
parent_frame_id,
child1_frame_id,
radius=1,
time=1):
rate = rospy.Rate(int(self.pub_freq))
for i in range(int(self.pub_freq * time) + 1):
t = i / self.pub_freq / time
self.br.sendTransform(
(-radius * math.cos(2 * math.pi * t) + radius,
-radius * math.sin(2 * math.pi * t), 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), child1_frame_id, parent_frame_id)
rate.sleep()
class Application:
#rospy.Subscriber("/rosout_agg", Log, self.state_transition_cb, queue_size=None)
def __init__(self):
# initialize the ATF class
self.atf = ATF()
def state_transition_cb(self, msg):
# TODO sm feedback on state change [eg: ERROR, READY, BUSY ...]
if 'subscribed to person_of_interest_filter' in msg.msg:
self.ready_for_scenario = True
def execute(self):
# you can call start/pause/purge/stop for each testblock during the execution of your app
rospy.sleep(5)
#self.atf.start("testblock_all")
self.atf.start("testblock_small")
# Do something
#self.wait_for_robot_ready();
#self.atf.stop("testblock_1")
#self.atf.start("testblock_2")
# Do something else
rospy.sleep(200)
self.atf.stop("testblock_small")
#self.atf.stop("testblock_all")
# finally we'll have to call shutdown() to tell the ATF to stop all recordings and wrap up
self.atf.shutdown()
def wait_for_robot_ready(self):
rospy.loginfo("wait for robot to be ready")
r = rospy.Rate(1)
while not rospy.is_shutdown():
if self.ready_for_scenario:
break
r.sleep()
class Application:
def __init__(self):
self.pub = rospy.Publisher(
'/gazebo/set_model_state', ModelState,
queue_size=1) # gazebo move object publisher
self.name = 'box1' # name of object to spawn
self.testcases = [
'line_passage', # 0
'line_passage_obstacle', # 1
'line_passage_person_moving', # 2
'line_passage_spawn_obstacle', # 3
'narrow_passage_2_cone', # 4
't_passage', # 5
't_passage_obstacle'
] # 6
self.rospack = rospkg.RosPack() # get path for ROS package
rp = RvizPublisher()
# filepath = self.rospack.get_path('msh_bringup') + '/launch/' + self.args.launch + '.launch'
if "standalone" == sys.argv[1]:
filepath = self.rospack.get_path(
'msh_bringup') + '/launch/' + sys.argv[2] + '.launch'
else:
filepath = self.rospack.get_path(
'msh_bringup') + '/launch/' + self.testcases[0] + '.launch'
print '=' * len(filepath)
print filepath
print '=' * len(filepath)
rp.main(filepath, True, False) # 'initialpose' publish
del rp
rospy.sleep(1)
# setup threading daemon to beam object in front of robot
self.beam_daemon = threading.Timer(15, self.beam_object,
[5.0, -0.5, 0.0, 0.0, 0.0])
self.beam_daemon.setDaemon(True)
self.beam_daemon.setName('beam_daemon')
self.atf = ATF()
def execute(self):
self.atf.start('testblock_nav')
self.beam_daemon.start()
# necessary to catch goal published on topic /move_base/goal
rospy.sleep(3)
# line passage goal
sss.move("base", [7.0, 0.0, 0.0])
self.atf.stop('testblock_nav')
self.atf.shutdown()
def beam_object(self, x, y, roll, pitch, yaw):
'''
beams an available object to the desired position
:param x: x-Position in [m]
:param y: y-Position in [m]
:param roll: Euler angle transformation
:param pitch: Euler angle transformation
:param yaw: Euler angle transformation
:return: --
'''
# gazebo takes ModelState as msg type
model_state = ModelState()
model_state.model_name = self.name
model_state.reference_frame = 'world'
model_state.pose.position.x = x
model_state.pose.position.y = y
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
model_state.pose.orientation.x = quaternion[0]
model_state.pose.orientation.y = quaternion[1]
model_state.pose.orientation.z = quaternion[2]
model_state.pose.orientation.w = quaternion[3]
rospy.loginfo('\033[92m' + '----Move Object Gazebo----' + '\033[0m')
# publish message
self.pub.publish(model_state)
class Application:
def __init__(self):
self.atf = ATF()
moveit_commander.roscpp_initialize(sys.argv)
self.group = moveit_commander.MoveGroupCommander("manipulator")
def execute(self):
# dummy home
self.atf.start("initialize")
pose_target = geometry_msgs.msg.Pose()
pose_target.position.x = 0.732
pose_target.position.y = 0.0
pose_target.position.z = 1.325
pose_target.orientation.w = 1.0
self.group.set_pose_target(pose_target)
self.group.go()
self.atf.stop("initialize")
self.atf.start("full_test")
# down
self.atf.start("move_down")
pose_target2 = geometry_msgs.msg.Pose()
pose_target2.position.x = 1.140
pose_target2.position.y = 0.0
pose_target2.position.z = 0.825
pose_target2.orientation.w = 1.0
self.group.set_pose_target(pose_target2)
self.group.go()
self.atf.stop("move_down")
# left
self.atf.start("move_left")
pose_target3 = geometry_msgs.msg.Pose()
pose_target3.position.x = 0.0
pose_target3.position.y = 1.140
pose_target3.position.z = 0.825
pose_target3.orientation.w = 1.0
self.group.set_pose_target(pose_target3)
self.group.go()
self.atf.stop("move_left")
# right
self.atf.start("move_right")
pose_target4 = geometry_msgs.msg.Pose()
pose_target4.position.x = 0.0
pose_target4.position.y = -1.140
pose_target4.position.z = 0.825
pose_target4.orientation.w = 1.0
self.group.set_pose_target(pose_target4)
self.group.go()
self.atf.stop("move_right")
# home
self.atf.start("move_up")
pose_target5 = geometry_msgs.msg.Pose()
pose_target5.position.x = 0.732
pose_target5.position.y = 0.0
pose_target5.position.z = 1.325
pose_target5.orientation.w = 1.0
self.group.set_pose_target(pose_target5)
self.group.go()
self.atf.stop("move_up")
self.atf.stop("full_test")
self.atf.shutdown()