def callback_state(self, data):
for idx, cube in enumerate(data.name):
self.cubes_state.setdefault(cube, [0] * 3)
pose = self.cubes_state[cube]
cube_init = self.CubeMap[cube]["init"]
pose[0] = data.pose[idx].position.x + cube_init[0]
pose[1] = data.pose[idx].position.y + cube_init[1]
pose[2] = data.pose[idx].position.z + cube_init[2]
# def add_cube(self, name):
# p = PoseStamped()
# p.header.frame_id = ros_robot.get_planning_frame()
# p.header.stamp = rospy.Time.now()
#
# # p.pose = self._arm.get_random_pose().pose
# p.pose.position.x = -0.18
# p.pose.position.y = 0
# p.pose.position.z = 0.046
#
# q = quaternion_from_euler(0.0, 0.0, 0.0)
# p.pose.orientation = Quaternion(*q)
# ros_scene.add_box(name, p, (0.02, 0.02, 0.02))
#
# def remove_cube(self, name):
# ros_scene.remove_world_object(name)
评论列表
文章目录