Add Navigation Stack to Stage Simulation

.gitignore

@@ -2,9 +2,13 @@
 build/
 devel/
 
+# Devel directory
+amp-devel/
+
 # AMP-CLI files
-__pycache__/
-AMP_CLI.egg-info/
+*.egg-info
+__pycache__
+logs/
 
 # Catkin ignore files
 CATKIN_IGNORE

README.md

@@ -3,22 +3,21 @@
 
 Second iteration of the Autonomous Software Stack (ASS) for the AMP go-kart.
 
-# Current Nodes
-## stage_ros
-See sim\_files inside the `env_sim` file for the image file that specifies the shape of the racetrack and the config files that define the simulated laserscanner sensor and kart.
+# Design
+Our software stack uses the [ROS 2d navigation package](http://wiki.ros.org/navigation) to obtain control information given sensor and odometry sources, a map, a [tf](http://wiki.ros.org/tf) transform tree, a node to publish the desired kart position (i.e. goal setting), and a set of configurations for the naviagtion stack's global and local planners and costmaps.
+This repository contains top-level launch files for simulation and physical testing in `src/sim_stage/launch` and `.../launch` (TODO: Issue 34). We highlight some of the main packages below:
 
-This simulated kart takes /cmd\_vel topic to move. (cmd\_vel contains tf map frame, quaternion, and 3d pose, but we are only using it for forward displacement and yaw displacement)   
+TODO(35): Add other packages and refactor current ones.
 
+## sim_stage
+This package runs the [`stage_ros`](http://wiki.ros.org/stage_ros) node. See `src/sim_stage/sim_files` for the image file that specifies the shape of the racetrack and the config files that define the simulated laserscanner sensor and kart.
+This simulated kart takes /cmd\_vel topic to move. (cmd\_vel contains tf map frame, quaternion, and 3d pose, but we are only using it for forward displacement and yaw displacement)   
 Broadcasts /top/scan, which is the current moment laserscan data as a list of each ray's distance in a 180 degree range. 
 
 ## slam_mode_goal
 Reads top/scan and moves the kart towards open space (path finding). Puts out /amp\_slam\_goal which is a MoveBaseGoal structure that represents displacement. TODO: change from MoveBaseGoal to something simpler.
-
 slam\_mode\_goal is intended to put out high-level, ideal cart commands that are translated by the mover node into hardware commands for the kart's motors and steering.
 
-## Mover
-Takes MoveBaseGoal and converts them to cmd_vel to the simulated kart
-
 # Running 
 * To build the amp-cli tool, run:
 ```

amp_cli.py

@@ -5,6 +5,15 @@
 import click
 
 
+def write_log_to_file(file_name, log_generator):
+    with open(file_name, 'w') as f:
+        for log in log_generator:
+            try:
+                f.write(log['stream'])
+            except:
+                pass
+
+
 @click.group()
 def cli():
     """This is the official AMP Software Sub-Team CLI Tool that helps you 
@@ -31,7 +40,7 @@ def devel(build: bool, display: str):
         client = docker.from_env()
     except Exception as exception:
         raise click.ClickException(
-            f'Ran into an error, look at the help menu.\nError: {exception}')
+            f'Error: {exception}\nRan into an error, look at the help menu.')
     cwd = os.getcwd()
     tag = 'amp-devel:frame-desktop'
 
@@ -41,31 +50,40 @@ def devel(build: bool, display: str):
             'Devel only works when executed from the root directory of the AMP_ASSv2 local repo.', fg='red'))
 
     if build:
+        subprocess.run(['bash', '-c', 'mkdir -p logs'])
         click.echo('Building containers...')
 
         click.echo('Step 1/2: Building amp-devel:noetic-desktop.')
         try:
-            _ = client.images.build(
+            _, logs = client.images.build(
                 path=cwd, tag='amp-devel:noetic-desktop',
                 dockerfile=(cwd+'/docker/desktop.Dockerfile'))
             click.secho(
                 ' ---> Finished building amp-devel:noetic-desktop.', fg='yellow')
         except Exception as exception:
             raise click.ClickException(
-                f'Ran into an error, look at the help menu.\nError: {exception}')
+                f'Error: {exception}\nRan into an error, look at the help menu.')
+        write_log_to_file("logs/devel-build-desktop.log", logs)
+        click.secho(
+            ' ---> Finished saving logs.', fg='yellow')
 
         click.echo(f'Step 2/2: Building {tag}.')
         try:
-            _ = client.images.build(
+            _, logs = client.images.build(
                 path=cwd, tag=tag,
                 dockerfile=(cwd+'/docker/frame.Dockerfile'))
             click.secho(
                 f' ---> Finished building {tag}.', fg='yellow')
         except Exception as exception:
             raise click.ClickException(
-                f'Ran into an error, look at the help menu.\nError: {exception}')
+                f'Error: {exception}\nRan into an error, look at the help menu.')
+        write_log_to_file("logs/devel-build-frame.log", logs)
+        click.secho(
+            ' ---> Finished saving logs.', fg='yellow')
 
     try:
+        subprocess.run(['bash', '-c', 'mkdir -p amp-devel/build'])
+        subprocess.run(['bash', '-c', 'mkdir -p amp-devel/devel'])
         # Run the contianer
         click.echo(f'Running {tag}...')
         if display == 'mesa':
@@ -83,8 +101,16 @@ def devel(build: bool, display: str):
                         'bind': '/tmp/.X11-unix',
                         'mode': 'rw',
                     },
-                    cwd: {
-                        'bind': '/amp_ws',
+                    cwd+'/src': {
+                        'bind': '/amp_ws/src',
+                        'mode': 'rw',
+                    },
+                    cwd+'/amp-devel/build': {
+                        'bind': '/amp_ws/build',
+                        'mode': 'rw',
+                    },
+                    cwd+'/amp-devel/devel': {
+                        'bind': '/amp_ws/devel',
                         'mode': 'rw',
                     },
                 },
@@ -95,18 +121,22 @@ def devel(build: bool, display: str):
             raise click.ClickException('NVidia config WIP')
         # Attach to the container
         click.echo('Use [<Ctrl> + D] to stop and exit the container.')
+        click.secho(
+            'Note: Rosdep packages are not installed in devel', fg='yellow')
+        click.secho(
+            'Run "apt update && rosdep install --from-paths src -iry"', fg='yellow')
         click.secho(
             f'Entering container {container.name}:{container.short_id}.', fg='blue')
-        subprocess.run(['bash', '-c', 'xhost +local:docker'])
+        subprocess.run(['bash', '-c', 'xhost +'])
         subprocess.run(['bash', '-c', f'docker attach {container.id}'])
-        subprocess.run(['bash', '-c', 'xhost -local:docker'])
+        subprocess.run(['bash', '-c', 'xhost -'])
         click.secho('Successfully exited and stopped container.', fg='green')
     except docker.errors.ImageNotFound:
         raise click.ClickException(
             f'Unable to find {tag}, maybe try running with "--build" flag?')
     except Exception as exception:
         raise click.ClickException(
-            f'Ran into an error, look at the help menu.\nError {exception}')
+            f'Error: {exception}\nRan into an error, look at the help menu.')
 
 
 @cli.command()
@@ -123,7 +153,7 @@ def scratch(build: bool, display: str):
         client = docker.from_env()
     except Exception as exception:
         raise click.ClickException(
-            f'Ran into an error, look at the help menu.\nError: {exception}')
+            f'Error: {exception}\nRan into an error, look at the help menu.')
     cwd = os.getcwd()
 
     tag = f'amp-devel:{display}-build'
@@ -136,38 +166,48 @@ def scratch(build: bool, display: str):
             raise click.ClickException(click.style(
                 'Build only works inside root directory of the AMP_ASSv2 local repository.', fg='red'))
 
+        subprocess.run(['bash', '-c', 'mkdir -p logs'])
         click.echo('Building containers...')
 
         click.echo('Step 1/3: Building amp-devel:noetic-desktop.')
         try:
-            _ = client.images.build(
+            _, logs = client.images.build(
                 path=cwd, tag='amp-devel:noetic-desktop',
                 dockerfile=(cwd+'/docker/desktop.Dockerfile'))
             click.secho(
                 ' ---> Finished building amp-devel:noetic-desktop.', fg='yellow')
         except Exception as exception:
             raise click.ClickException(
-                f'Ran into an error, look at the help menu.\nError {exception}')
+                f'Error: {exception}\nRan into an error, look at the help menu.')
+        write_log_to_file("logs/scratch-build-desktop.log", logs)
+        click.secho(
+            ' ---> Finished saving logs.', fg='yellow')
 
         click.echo('Step 2/3: Building amp-devel:frame-desktop.')
         try:
-            _ = client.images.build(
+            _, logs = client.images.build(
                 path=cwd, tag='amp-devel:frame-desktop',
                 dockerfile=(cwd+'/docker/frame.Dockerfile'))
             click.secho(
                 ' ---> Finished building amp-devel:frame-desktop.', fg='yellow')
         except Exception as exception:
             raise click.ClickException(
-                f'Ran into an error, look at the help menu.\nError {exception}')
+                f'Error: {exception}\nRan into an error, look at the help menu.')
+        write_log_to_file("logs/scratch-build-frame.log", logs)
+        click.secho(
+            ' ---> Finished saving logs.', fg='yellow')
 
         click.echo(f'Step 3/3: Building {tag}.')
         try:
-            _ = client.images.build(
+            _, logs = client.images.build(
                 path=cwd, tag=tag, dockerfile=dockerfile)
             click.secho(f' ---> Finished building {tag}.', fg='yellow')
         except Exception as exception:
             raise click.ClickException(
-                f'Ran into an error, look at the help menu.\nError {exception}')
+                f'Error: {exception}\nRan into an error, look at the help menu.')
+        write_log_to_file(f"logs/scratch-build-{display}.log", logs)
+        click.secho(
+            ' ---> Finished saving logs.', fg='yellow')
 
     try:
         # Run the contianer
@@ -197,14 +237,14 @@ def scratch(build: bool, display: str):
         click.echo('Use [<Ctrl> + D] to stop and exit the container.')
         click.secho(
             f'Entering container {container.name}:{container.short_id}.', fg='blue')
-        subprocess.run(['bash', '-c', 'xhost +local:docker'])
+        subprocess.run(['bash', '-c', 'xhost +'])
         subprocess.run(['bash', '-c', f'docker attach {container.id}'])
-        subprocess.run(['bash', '-c', 'xhost -local:docker'])
+        subprocess.run(['bash', '-c', 'xhost -'])
         click.secho('Successfully exited and stopped container.', fg='green')
 
     except docker.errors.ImageNotFound:
         raise click.ClickException(
             f'Unable to find {tag}, maybe try running with "--build" flag?')
     except Exception as exception:
         raise click.ClickException(
-            f'Ran into an error, look at the help menu.\nError {exception}')
+            f'Error: {exception}\nRan into an error, look at the help menu.')

docker/frame.Dockerfile

@@ -2,6 +2,8 @@ FROM amp-devel:noetic-desktop
 SHELL ["/bin/bash", "-c"]
 WORKDIR /amp_ws
 
+COPY .catkin_workspace .
+
 RUN apt-get update && apt-get install -q -y --no-install-recommends \
     tmux \
     curl \
@@ -12,7 +14,6 @@ RUN apt-get update && apt-get install -q -y --no-install-recommends \
     mesa-utils \
     unzip \ 
     && rm -rf /var/lib/apt/list/* \
-    && rosdep update
 
 ENTRYPOINT ["/ros_entrypoint.sh"]
 CMD ["bash"]

docker/mesa.Dockerfile

@@ -3,10 +3,12 @@ SHELL ["/bin/bash", "-c"]
 WORKDIR /amp_ws
 
 COPY src ./src
-COPY .catkin_workspace .
 
-RUN . /opt/ros/noetic/setup.bash && \
-    rosdep install --from-paths src -r -y && \
+RUN apt-get update && \
+    rosdep update && \
+    . /opt/ros/noetic/setup.bash && \
+    rosdep install --from-paths src -iry && \
+    rm -rf /var/lib/apt/list/* && \
     catkin_make
 RUN echo "source /amp_ws/devel/setup.bash" >> ~/.bashrc
 

setup.py

@@ -2,7 +2,7 @@
 
 setup(
     name='AMP-CLI',
-    version='0.1',
+    version='0.2',
     py_modules=['amp_cli'],
     install_requires=[
         'click',