# Grasp pose for first wall
grasp_pose = copy.deepcopy(grasp_approach_pose)
grasp_pose.position.z = grasp_z - 0.05
my_cartesian_move(grasp_pose, group_placer)
rospy.sleep(1)
# Close the gripper
pub_gripper_p.publish(gripper_close_dist)
pub_gripper_d.publish(-gripper_close_dist)
rospy.sleep(1)
# Add wall mesh to planning scene and attach to robot
p = PoseStamped()
p.header.frame_id = 'placer_tool0'
p.pose = Pose(Point(0, 0, 0.6996), Quaternion(0, -1, 0, 0))
scene.attach_mesh(
'placer_tool0',
'wall_1',
p,
'/home/mqm/.gazebo/models/wall_plain_with_blocking/meshes/wall_plain_with_blocking.stl',
size=(.025, .025, .025),
touch_links=['side_d', 'side_p', 'footer_1_simple'])
# clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
# sys.exit(0)
rospy.sleep(2)
# Lift the wall
my_cartesian_move(grasp_approach_pose, group_placer)
# Approach pose for placing first wall
