def test_laser_constraint_real_laser_avoidance(self, kitchen_setup):
goalpose = PoseStamped()
goalpose.pose.position = Point(0, 0, 0)
goalpose.pose.orientation = Quaternion(
*quaternion_about_axis(np.pi / 2, [0, 0, 1]))
goalpose.header.frame_id = "map"
kitchen_setup.move_base(goalpose)
kitchen_setup.add_json_goal(u'LaserAvoidance')
goal_pose = PoseStamped()
goal_pose.pose.position = Point(-4, 4, 0)
goal_pose.pose.orientation = Quaternion(
*quaternion_about_axis(np.pi / 2, [0, 0, 1]))
goal_pose.header.frame_id = "map"
js = {
u'odom_x_joint': goal_pose.pose.position.x,
u'odom_y_joint': goal_pose.pose.position.y
}
kitchen_setup.set_cart_goal(goal_pose,
"base_footprint",
kitchen_setup.default_root,
weight=WEIGHT_COLLISION_AVOIDANCE)
kitchen_setup.allow_all_collisions()
kitchen_setup.send_and_check_goal()
expected = tf.lookup_point("map", "base_footprint")
np.testing.assert_almost_equal(expected.point.y, 4, 1)
np.testing.assert_almost_equal(expected.point.z, 0, 1)
def test_full_coverage_non_simulated_avoidance(self, kitchen_setup):
goalpose = PoseStamped()
goalpose.pose.position = Point(0, 0, 0)
goalpose.pose.orientation = Quaternion(*quaternion_about_axis(-np.pi / 2, [0, 0, 1]))
goalpose.header.frame_id = "map"
kitchen_setup.move_base(goalpose)
rospy.sleep(1)
for i in range(10):
kitchen_setup.add_json_goal(u'LaserCollisionAvoidance', idx=i)
goal_pose = PoseStamped()
goal_pose.pose.position = Point(2.5, -7.5, 0)
goal_pose.pose.orientation = Quaternion(*quaternion_about_axis(-np.pi / 2, [0, 0, 1]))
goal_pose.header.frame_id = "map"
js = {u'odom_x_joint': goal_pose.pose.position.x,
u'odom_y_joint': goal_pose.pose.position.y}
kitchen_setup.set_cart_goal(goal_pose, "base_footprint", kitchen_setup.default_root,
weight=WEIGHT_COLLISION_AVOIDANCE)
# kitchen_setup.add_json_goal(js)
kitchen_setup.allow_all_collisions()
kitchen_setup.send_and_check_goal()
expected = tf.lookup_point("map", "base_footprint")
print(expected)
np.testing.assert_almost_equal(expected.point.y, 2, 1)
np.testing.assert_almost_equal(expected.point.z, 0, 1)
def test_pointing(self, better_pose):
tip = u'rs_camera_link'
goal_point = tf.lookup_point(u'map', u'base_footprint')
better_pose.wrapper.pointing(tip, goal_point)
better_pose.send_and_check_goal()
current_x = Vector3Stamped()
current_x.header.frame_id = tip
current_x.vector.z = 1
expected_x = tf.transform_point(tip, goal_point)
np.testing.assert_almost_equal(expected_x.point.y, 0, 2)
np.testing.assert_almost_equal(expected_x.point.x, 0, 2)
def test_pointing(self, better_pose):
tip = u'head_mount_kinect2_rgb_optical_frame'
goal_point = lookup_point(u'map', better_pose.r_tip)
better_pose.pointing(tip, goal_point)
better_pose.send_and_check_goal()
current_x = Vector3Stamped()
current_x.header.frame_id = tip
current_x.vector.z = 1
expected_x = transform_point(tip, goal_point)
np.testing.assert_almost_equal(expected_x.point.y, 0, 2)
np.testing.assert_almost_equal(expected_x.point.x, 0, 2)
goal_point = lookup_point(u'map', better_pose.r_tip)
better_pose.pointing(tip, goal_point, root=better_pose.r_tip)
r_goal = PoseStamped()
r_goal.header.frame_id = better_pose.r_tip
r_goal.pose.position.x -= 0.2
r_goal.pose.position.z -= 0.5
r_goal.pose.orientation.w = 1
r_goal = transform_pose(better_pose.default_root, r_goal)
r_goal.pose.orientation = Quaternion(*quaternion_from_matrix(
[[0, 0, 1, 0], [0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 0, 1]]))
better_pose.set_and_check_cart_goal(r_goal, better_pose.r_tip,
u'base_footprint')
current_x = Vector3Stamped()
current_x.header.frame_id = tip
current_x.vector.z = 1
expected_x = lookup_point(tip, better_pose.r_tip)
np.testing.assert_almost_equal(expected_x.point.y, 0, 2)
np.testing.assert_almost_equal(expected_x.point.x, 0, 2)
def test_laser_constraint_1(self, kitchen_setup):
goalpose = PoseStamped()
goalpose.pose.position = Point(0, -1, 0)
goalpose.pose.orientation.w = 1
goalpose.header.frame_id = "map"
boxpose = PoseStamped()
boxpose.pose.position = Point(0, 0.5, 0)
boxpose.pose.orientation.w = 1
boxpose.header.frame_id = "map"
kitchen_setup.move_base(goalpose)
kitchen_setup.add_box(u'nursoda', (0.1, 0.1, 0.5), boxpose)
for i in range(10):
kitchen_setup.add_json_goal(u'LaserCollisionAvoidance', idx=i)
kitchen_setup.allow_all_collisions()
kitchen_setup.send_and_check_goal()
expected_x = tf.lookup_point("map", "base_footprint")
np.testing.assert_almost_equal(expected_x.point.y, 0, 1)
np.testing.assert_almost_equal(expected_x.point.z, 0, 1)
def full_coverage_avoidance(self, start_position, goal_position, kitchen_setup):
kitchen_setup.move_base(start_position)
rospy.sleep(1)
x = raw_input("Press any Key to start")
#execfile('/home/philipp/bachelorarbeit/bp_ws/src/giskardpy/scripts/laser_scan_longtime_visualizer.py')
#th = Thread(lv.main())
#th.start()
# kitchen_setup.add_cylinder(u'nursoda' + str(i), [1, 0.5], cylinderpose)
for i in range(10):
kitchen_setup.add_json_goal(u'LaserCollisionAvoidance', idx=i)
kitchen_setup.set_cart_goal(goal_position, "base_footprint", kitchen_setup.default_root,
weight=WEIGHT_COLLISION_AVOIDANCE*2)
kitchen_setup.allow_all_collisions()
kitchen_setup.send_and_check_goal()
expected = tf.lookup_point("map", "base_footprint")
np.testing.assert_almost_equal(expected.point.x, goal_position.pose.position.x, 1)
np.testing.assert_almost_equal(expected.point.y, goal_position.pose.position.y, 1)
np.testing.assert_almost_equal(expected.point.z, 0, 1)
def test_laser_constraint_multiple_avoidance(self, kitchen_setup):
goalpose = PoseStamped()
goalpose.pose.position = Point(0, 0, 0)
goalpose.pose.orientation = Quaternion(
*quaternion_about_axis(0, [0, 0, 1]))
goalpose.header.frame_id = "map"
kitchen_setup.move_base(goalpose)
boxes = [Point(1, 2, 0.46), Point(-1, 2, 0.46), Point(0.3, 0.9, 0.46)]
for i in range(len(boxes)):
boxpose = PoseStamped()
boxpose.pose.position = boxes[i]
boxpose.pose.orientation.w = 1
boxpose.header.frame_id = "map"
kitchen_setup.add_box(u'nursoda' + str(i), (0.3, 0.1, 0.9),
boxpose)
for i in range(10):
kitchen_setup.add_json_goal(u'LaserCollisionAvoidance', idx=i)
goal_pose = PoseStamped()
goal_pose.pose.position = Point(0, 2, 0)
goal_pose.pose.orientation = Quaternion(
*quaternion_about_axis(np.pi / 2, [0, 0, 1]))
goal_pose.header.frame_id = "map"
kitchen_setup.set_cart_goal(goal_pose,
"base_footprint",
kitchen_setup.default_root,
weight=WEIGHT_COLLISION_AVOIDANCE)
kitchen_setup.allow_all_collisions()
kitchen_setup.send_and_check_goal()
expected = tf.lookup_point("map", "base_footprint")
print(expected)
np.testing.assert_almost_equal(expected.point.y, 2, 1)
np.testing.assert_almost_equal(expected.point.z, 0, 1)