Pose Estimation ROS Workspace

This repository is structured as a Catkin/ROS workspace containing a single Catkin package (object_detection_msgs) used to build ROS service messages. Alongside the typical Catkin folder structure, we have changed the repository structure as little as possible from the original setup.

NOTE: The following commands have not been verified to work until this comment is removed.

Setup Steps

1. Get the Docker Image (only once)

If you do not have the Docker image, you can pull it using the command:

docker pull lhy0807/haptic-refiner:ros1

If for some reason you cannot pull the image, you may be able to build the image only if your machine has much more than 32 GB of RAM (it has worked with 128 GB RAM but we do not know if 64 GB would be enough). With this in mind, as a last resort, you can build the image by running:

sh docker/build_ros1.sh

2. Enter Docker (every session)

To enter the Docker container, run:

sh docker/run_container_ros1.sh

3. Build FoundationPose (only once)

If it's the first time you've launched the container, you need to build FoundationPose. Run this command inside the Docker container:

sh /docker/pose/FoundationPose/build_all.sh

You'll also need to download the network weights for FoundationPose. Run the command:

sh /docker/pose/docker/download_weights.sh

4. Generate CNOS Templates (once per object)

We use CNOS for object segmentation as a preprocessing step for pose estimation. CNOS requires a CAD model of each novel object, used to render "templates" of the object from various views.

Because we couldn't get the CNOS template rendering to run locally, when we scan new object mesh files (.obj), send them to Hongyu to get CNOS templates. You can convert .usdz files to .obj online using sites such as this or this.

5. Build the ROS Workspace (only once)

To build the ROS messages needed for pose estimation, run:

pip install empy==3.3.4
cd /docker/pose/pose_ws
catkin_make

Proof-of-Life ROS Test

The run_spot.py script verifies that the pose estimation system can receive data from the robot. You'll need two Terminal tabs, one in the pose Docker and one in the spot-tamp-gpu Docker (from spot_skills). Follow these steps:

1. Verify Ethernet Connection - Verify that you can communicate with Spot over Ethernet from within Docker. Note that the example IP address corresponds to Snouter via Ethernet (other Spots will vary):

root@terminist:/docker/spot_skills# ping 138.16.160.90
PING 138.16.160.90 (138.16.160.90) 56(84) bytes of data.
64 bytes from 138.16.160.90: icmp_seq=1 ttl=64 time=0.374 ms
...
^C
--- 138.16.160.90 ping statistics ---
9 packets transmitted, 9 received, 0% packet loss, time 8186ms

2. Launch Spot ROS Driver - Enter the spot_skills Docker and launch the Spot ROS driver with RViz enabled:

roslaunch spot_skills authenticate_spot_driver.launch rviz:=true load_robot_description:=true

You should see the Spot in RViz reflect the configuration of the physical Spot, such as:

3. Launch Pose Estimator - Finally, in the pose Docker, run the following commands:

cd /docker/pose/real
source ../pose_ws/devel/setup.bash
python run_spot.py

Pose Estimation Service

The pose_estimation_server.py script provides the pose estimation service to be used by the rest of our codebase over in spot_skills. To run the service, open two Terminal tabs, one in the pose Docker and the other in spot-tamp-gpu (from spot_skills). Follow these steps:

1. Verify Ethernet Connection - Verify that you can communicate with Spot over Ethernet from within Docker. Note that the example IP address below is for Snouter; other Spots will use different IP addresses.

   root@terminist:/docker/spot_skills# ping 138.16.160.90
   PING 138.16.160.90 (138.16.160.90) 56(84) bytes of data.
   64 bytes from 138.16.160.90: icmp_seq=1 ttl=64 time=0.374 ms
   ...
   ^C
   --- 138.16.160.90 ping statistics ---
   9 packets transmitted, 9 received, 0% packet loss, time 8186ms
   

2. Launch the pose estimation service client from the spot_skills Docker:

   source devel/setup.bash
   roslaunch spot_skills pose_estimation_demo.launch

3. Launch the pose estimation service server from the pose Docker:

   sh /docker/pose/run_service.sh