fixed more bugs

SemRoCo · Oct 18, 2021 · 72c6a71 · 72c6a71
1 parent 9fbd43c
commit 72c6a71
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 13 deletions.
diff --git a/src/giskardpy/tree/world_updater.py b/src/giskardpy/tree/world_updater.py
@@ -230,6 +230,7 @@ def remove_object(self, name):
  name = str(PrefixName(name, 'js'))
  if name in tree.tree_nodes:
  tree.remove_node(name)
+ self.added_plugin_names.remove(name)
  logging.loginfo('Deleted {}'.format(name))
 
  def clear_world(self):

diff --git a/test/test_integration_pr2.py b/test/test_integration_pr2.py
@@ -2801,7 +2801,7 @@ def test_corrupt_shape_error(self, zero_pose):
  :type zero_pose: PR2
  """
  p = PoseStamped()
- p.header.frame_id = 'map'
+ p.header.frame_id = 'base_link'
  req = UpdateWorldRequest(UpdateWorldRequest.ADD, DEFAULT_WORLD_TIMEOUT,
  WorldBody(type=WorldBody.PRIMITIVE_BODY,
  shape=SolidPrimitive(type=42)), True, p)
@@ -2813,7 +2813,7 @@ def test_tf_error(self, zero_pose):
  """
  req = UpdateWorldRequest(UpdateWorldRequest.ADD, DEFAULT_WORLD_TIMEOUT,
  WorldBody(type=WorldBody.PRIMITIVE_BODY,
- shape=SolidPrimitive(type=42)), True, PoseStamped())
+ shape=SolidPrimitive(type=42)), False, PoseStamped())
  assert zero_pose._update_world_srv.call(req).error_codes == UpdateWorldResponse.TF_ERROR
 
  def test_unsupported_options(self, kitchen_setup):
@@ -2823,7 +2823,7 @@ def test_unsupported_options(self, kitchen_setup):
  wb = WorldBody()
  pose = PoseStamped()
  pose.header.stamp = rospy.Time.now()
- pose.header.frame_id = str(u'map')
+ pose.header.frame_id = str(u'base_link')
  pose.pose.position = Point()
  pose.pose.orientation = Quaternion(w=1)
  wb.type = WorldBody.URDF_BODY
@@ -2920,8 +2920,8 @@ def test_allow_self_collision(self, zero_pose):
  """
  :type zero_pose: PR2
  """
- zero_pose.check_cpi_geq(zero_pose.get_r_gripper_links(), 0.1)
- zero_pose.check_cpi_geq(zero_pose.get_l_gripper_links(), 0.1)
+ zero_pose.check_cpi_geq(zero_pose.get_r_gripper_links(), 0.05)
+ zero_pose.check_cpi_geq(zero_pose.get_l_gripper_links(), 0.05)
 
  def test_allow_self_collision2(self, zero_pose):
  """
@@ -2979,7 +2979,7 @@ def test_allow_self_collision3(self, zero_pose):
  ces = []
  ces.append(CollisionEntry(type=CollisionEntry.ALLOW_COLLISION,
  robot_links=zero_pose.get_l_gripper_links(),
- body_b=u'pr2',
+ body_b=u'robot',
  link_bs=zero_pose.get_r_forearm_links()))
  zero_pose.set_collision_entries(ces)
 

diff --git a/test/utils_for_tests.py b/test/utils_for_tests.py
@@ -797,9 +797,11 @@ def check_cpi_geq(self, links, distance_threshold):
  for link in links:
  collisions = self.get_external_collisions(link, distance_threshold)
  assert collisions[0].contact_distance >= distance_threshold, \
- u'distance for {}: {} >= {}'.format(link,
- collisions[0].contact_distance,
- distance_threshold)
+ u'distance for {}: {} >= {} ({} with {})'.format(link,
+ collisions[0].contact_distance,
+ distance_threshold,
+ collisions[0].original_link_a,
+ collisions[0].original_link_b)
 
  def check_cpi_leq(self, links, distance_threshold):
  for link in links:
@@ -808,8 +810,8 @@ def check_cpi_leq(self, links, distance_threshold):
  u'distance for {}: {} <= {} ({} with {})'.format(link,
  collisions[0].contact_distance,
  distance_threshold,
- collisions[0].link_a,
- collisions[0].link_b)
+ collisions[0].original_link_a,
+ collisions[0].original_link_b)
 
  def move_base(self, goal_pose):
  """
@@ -839,12 +841,12 @@ def move_base(self, goal_pose):
  def get_l_gripper_links(self):
  if 'l_gripper' not in self.world.group_names:
  self.world.register_group('l_gripper', 'l_wrist_roll_link')
- return self.world.groups['l_gripper'].link_names_with_collisions
+ return [str(x) for x in self.world.groups['l_gripper'].link_names_with_collisions]
 
  def get_r_gripper_links(self):
  if 'r_gripper' not in self.world.group_names:
  self.world.register_group('r_gripper', 'r_wrist_roll_link')
- return self.world.groups['r_gripper'].link_names_with_collisions
+ return [str(x) for x in self.world.groups['r_gripper'].link_names_with_collisions]
 
  def get_r_forearm_links(self):
  return [u'r_wrist_flex_link', u'r_wrist_roll_link', u'r_forearm_roll_link', u'r_forearm_link',