diff --git a/.circleci/config.yml b/.circleci/config.yml
index f008db9b..b60844a9 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -3,7 +3,7 @@ jobs:
   build:
     # Setup docker
     docker:
-      - image: usdotfhwastol/carma-base:carma-system-4.1.0
+      - image: usdotfhwastol/carma-base:carma-system-4.2.0
         user: carma
         environment:
           TERM: xterm # use xterm to get full display output from build
diff --git a/Dockerfile b/Dockerfile
index b9c5f748..4eefdc4b 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -1,4 +1,4 @@
-FROM usdotfhwastol/carma-base:carma-system-4.1.0 as base_image
+FROM usdotfhwastol/carma-base:carma-system-4.2.0 as base_image
 SHELL ["/bin/bash", "-c"]
 
 ARG DEBIAN_FRONTEND="noninteractive"
@@ -13,42 +13,10 @@ LABEL org.label-schema.vcs-url="https://github.com/usdot-fhwa-stol/carma-msgs"
 LABEL org.label-schema.vcs-ref=${VCS_REF}
 LABEL org.label-schema.build-date=${BUILD_DATE}
 
-# Clone autoware repo to access messages
-RUN cd /home/carma/ && git clone https://github.com/usdot-fhwa-stol/autoware.ai.git --depth 1 --branch carma-system-4.1.0
-
-# ROS 1 msgs setup
-RUN mkdir -p ~/.base-image/ros1_msgs_ws/src/carma_msgs
+RUN  mkdir -p ~/.base-image/ros1_msgs_ws/src/carma_msgs
+RUN  mkdir -p /home/carma/src
 COPY . /home/carma/.base-image/ros1_msgs_ws/src/carma_msgs/
-RUN cp -R /home/carma/autoware.ai/messages /home/carma/.base-image/ros1_msgs_ws/src/autoware.ai/
-RUN cp -R /home/carma/autoware.ai/jsk_common_msgs /home/carma/.base-image/ros1_msgs_ws/src/autoware.ai/
-RUN cp -R /home/carma/autoware.ai/jsk_recognition /home/carma/.base-image/ros1_msgs_ws/src/autoware.ai/
-
-
-# ROS 2 msgs setup
-RUN mkdir -p ~/.base-image/ros2_msgs_ws/src/carma_msgs
+RUN  mkdir -p ~/.base-image/ros2_msgs_ws/src/carma_msgs
 COPY . /home/carma/.base-image/ros2_msgs_ws/src/carma_msgs/
-RUN mkdir -p ~/.base-image/ros2_msgs_ws/src/autoware.ai
-RUN cp -R /home/carma/autoware.ai/messages /home/carma/.base-image/ros2_msgs_ws/src/autoware.ai/
-RUN cp -R /home/carma/autoware.ai/jsk_common_msgs /home/carma/.base-image/ros2_msgs_ws/src/autoware.ai/
-RUN cp -R /home/carma/autoware.ai/jsk_recognition /home/carma/.base-image/ros2_msgs_ws/src/autoware.ai/
-
-# Cleanup autoware repo once messages have been moved
-RUN rm -r /home/carma/autoware.ai/
-
-# ROS1 message setup
-RUN cd ~/.base-image/ros1_msgs_ws && source /opt/ros/noetic/setup.bash && colcon build --packages-up-to autoware_msgs cav_msgs cav_srvs j2735_msgs can_msgs carma_debug_msgs autoware_lanelet2_msgs
-
-# ROS2 message setup
-RUN cd ~/.base-image/ros2_msgs_ws && source /opt/ros/foxy/setup.bash && colcon build --packages-up-to autoware_msgs autoware_lanelet2_msgs can_msgs carma_debug_ros2_msgs carma_driver_msgs carma_localization_msgs carma_msgs carma_perception_msgs carma_planning_msgs carma_v2x_msgs j2735_v2x_msgs
+RUN /home/carma/.base-image/ros1_msgs_ws/src/carma_msgs/docker/install.sh
 
-# Build the bridge
-RUN source /opt/ros/noetic/setup.bash \
-&& source /opt/ros/foxy/setup.bash \
-&& source ~/.base-image/ros1_msgs_ws/install/local_setup.bash \
-&& source ~/.base-image/ros2_msgs_ws/install/local_setup.bash \
-&& cd ~/.base-image/workspace/src \
-&& git clone --depth 1 --branch carma-system-4.1.0  https://github.com/usdot-fhwa-stol/ros1_bridge.git \
-&& cd ../ \
-&& sudo apt-get update \
-&& colcon build --packages-select ros1_bridge --cmake-args "--debug-output" \
-&& sudo chmod -R ugo+x ~/.base-image/workspace/install
diff --git a/carma_planning_msgs/CMakeLists.txt b/carma_planning_msgs/CMakeLists.txt
index 3210aef2..b98121ae 100644
--- a/carma_planning_msgs/CMakeLists.txt
+++ b/carma_planning_msgs/CMakeLists.txt
@@ -28,6 +28,7 @@ find_package(ament_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(carma_v2x_msgs REQUIRED)
 
 ## Generate messages 
 file(GLOB msg_files msg/*.msg)
@@ -52,6 +53,7 @@ rosidl_generate_interfaces(
       std_msgs
       builtin_interfaces
       geometry_msgs
+      carma_v2x_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)
diff --git a/carma_planning_msgs/msg/PlatooningInfo.msg b/carma_planning_msgs/msg/PlatooningInfo.msg
index e5c55fb0..ae6aeb61 100644
--- a/carma_planning_msgs/msg/PlatooningInfo.msg
+++ b/carma_planning_msgs/msg/PlatooningInfo.msg
@@ -42,3 +42,21 @@ float32  host_cmd_speed
 
 # Desired gap with leader
 float32  desired_gap
+
+# Actual gap with leader
+float32 actual_gap
+
+# The time headway summation of all predecessors   
+float32  current_predecessor_time_headway_sum
+
+# The speed of the preceding vehicle, in m/s.
+float32  predecessor_speed
+
+# The position of the preceding vehicle, in m.
+float32  predecessor_position
+
+# The status of gap creation.
+bool is_create_gap
+
+# Ratio to increase desired gap
+float32 create_gap_adjuster
diff --git a/carma_planning_msgs/package.xml b/carma_planning_msgs/package.xml
index 2aaf09bf..76ab7bb5 100644
--- a/carma_planning_msgs/package.xml
+++ b/carma_planning_msgs/package.xml
@@ -30,6 +30,7 @@
   <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
   <depend>builtin_interfaces</depend>
+  <depend>carma_v2x_msgs</depend>
   
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>
diff --git a/carma_planning_msgs/srv/SetActiveRoute.srv b/carma_planning_msgs/srv/SetActiveRoute.srv
index 7f45ba98..4724c68a 100644
--- a/carma_planning_msgs/srv/SetActiveRoute.srv
+++ b/carma_planning_msgs/srv/SetActiveRoute.srv
@@ -9,8 +9,18 @@
 #
 
 # Request
+# Enumeration values for the method being used to provide the route destination points
+uint8 choice
+
+uint8 ROUTE_ID=0
+uint8 DESTINATION_POINTS_ARRAY=1
+
 # The id of the route to select
 string route_id
+
+# The array of destination points to be used for route generation 
+carma_v2x_msgs/Position3D[] destination_points
+
 ---
 # Response
 # An enumeration representing a service execution error as defined in the route design document.
diff --git a/carma_v2x_msgs/msg/MobilityReason.msg b/carma_v2x_msgs/msg/MobilityReason.msg
new file mode 100644
index 00000000..a7f3cccf
--- /dev/null
+++ b/carma_v2x_msgs/msg/MobilityReason.msg
@@ -0,0 +1,18 @@
+# MobilityReason.msg
+#
+#
+
+# Variable which can be used to store a specific reason value.
+uint8 reason
+
+# enumeration values for mobility reason:
+
+uint8       UNKNOWN=0 
+uint8       ACCEPTED=1 
+uint8       SAFETY_VIOLATION=2 
+uint8       INSUFFICIENT_TIME=3
+uint8       PLAN_CONFLICT=4
+uint8       OTHERWISE_ENGAGED=5
+uint8       INSUFFICIENT_URGENCY=6
+uint8       PLAN_UNCLEAR=7
+uint8       OTHER=8
\ No newline at end of file
diff --git a/carma_v2x_msgs/msg/MobilityRepeat.msg b/carma_v2x_msgs/msg/MobilityRepeat.msg
new file mode 100644
index 00000000..3fbca201
--- /dev/null
+++ b/carma_v2x_msgs/msg/MobilityRepeat.msg
@@ -0,0 +1,12 @@
+# MobilityRepeat.msg
+#
+#
+
+# Variable which can be used to store a specific repeat value.
+uint8 repeat
+
+# enumeration values for mobility repeat:
+
+uint8       UNKNOWN=0 
+uint8       REQUEST_AGAIN=1 
+uint8       NEVER_REQUEST_AGAIN=2 
diff --git a/carma_v2x_msgs/msg/MobilityResponse.msg b/carma_v2x_msgs/msg/MobilityResponse.msg
index d034eef1..b4e15aa2 100644
--- a/carma_v2x_msgs/msg/MobilityResponse.msg
+++ b/carma_v2x_msgs/msg/MobilityResponse.msg
@@ -4,10 +4,19 @@
 # recevied a MobilityRequest from that CAV and decide to involve in that plan
 
 # standard header for all mobility messages
-carma_v2x_msgs/MobilityHeader  m_header
+carma_v2x_msgs/MobilityHeader   m_header
 
 # urgency of the current plan from 0 to 1000
-uint16                   urgency
+uint16                    urgency
 
 # the content of this response
-bool                     is_accepted
+bool                      is_accepted
+
+# type of the proposal being suggested to neighbors
+carma_v2x_msgs/PlanType plan_type
+
+# 
+carma_v2x_msgs/MobilityReason   reason         # OPTIONAL
+
+# Whether or not to repeat the MobilityRequest
+carma_v2x_msgs/MobilityRepeat   repeat         # OPTIONAL
\ No newline at end of file
diff --git a/carma_v2x_msgs/msg/PlanType.msg b/carma_v2x_msgs/msg/PlanType.msg
index dfbcc574..e2f114f0 100644
--- a/carma_v2x_msgs/msg/PlanType.msg
+++ b/carma_v2x_msgs/msg/PlanType.msg
@@ -12,6 +12,16 @@ uint8       CHANGE_LANE_LEFT = 1
 uint8       CHANGE_LANE_RIGHT = 2
 uint8       JOIN_PLATOON_AT_REAR = 3
 uint8       PLATOON_FOLLOWER_JOIN = 4
+uint8       JOIN_PLATOON_FROM_FRONT = 5
+uint8       PLATOON_FRONT_JOIN = 6
+uint8       CUT_IN_FROM_SIDE = 7
+uint8       PLATOON_CUT_IN_JOIN = 8
+uint8       STOP_CREATE_GAP = 9
+uint8       CUT_IN_FRONT_DONE = 10
+uint8       CUT_IN_MID_OR_REAR_DONE = 11
+uint8       PLATOON_DEPARTURE = 12
+uint8       DELETE_MEMBER = 13
+
 
 # ideas for future additions:
 # uint8       SEARCHING_FOR_PLATOON = 5
@@ -21,3 +31,6 @@ uint8       PLATOON_FOLLOWER_JOIN = 4
 # uint8       PASSING = 9 
 # pulling onto shoulder, need multi-lane change
 # join platoon in middle, merging platoons
+# MobilityPlanType.msg
+#
+#
diff --git a/carma_v2x_msgs/msg/TrafficControlDetail.msg b/carma_v2x_msgs/msg/TrafficControlDetail.msg
index 565189d2..5146656c 100644
--- a/carma_v2x_msgs/msg/TrafficControlDetail.msg
+++ b/carma_v2x_msgs/msg/TrafficControlDetail.msg
@@ -24,15 +24,17 @@
 #	lataffinity ENUMERATED {left, right},
 #	latperm SEQUENCE (SIZE(2)) OF ENUMERATED {none, permitted, passing-only, emergency-only},
 #	parking ENUMERATED {no, parallel, angled},
-#	minspeed INTEGER (0..1023), -- tenths of m/s
-#	maxspeed INTEGER (0..1023), -- tenths of m/s
-#	minhdwy INTEGER (0..2047), -- tenths of meters
-#	maxvehmass INTEGER (0..65535), -- kg
-#	maxvehheight INTEGER (0..127), -- tenths of meters
-#	maxvehwidth INTEGER (0..127), -- tenths of meters
-#	maxvehlength INTEGER (0..1023), -- tenths of meters
-#	maxvehaxles INTEGER (2..15),
-#	minvehocc INTEGER (1..15), 
+#	minspeed DOUBLE (0..1023), -- m/s
+#	maxspeed DOUBLE (0..1023), -- m/s
+#	minhdwy DOUBLE (0..2047), -- meters
+#	maxvehmass DOUBLE (0..65535), -- kg
+#	maxvehheight DOUBLE (0..127), -- meters
+#	maxvehwidth DOUBLE (0..127), -- meters
+#	maxvehlength DOUBLE (0..1023), -- meters
+#	maxvehaxles INTEGER (2..15), -- number of axles
+#	minvehocc INTEGER (1..15), -- vehicle passenger count
+#   maxplatoonsize INTEGER (1..63), -- vehicle count
+#   minplatoonhdwy DOUBLE (0..2047), -- meters
 #	...
 #}
 
@@ -61,6 +63,8 @@ uint8 MAXVEHWIDTH_CHOICE=16
 uint8 MAXVEHLENGTH_CHOICE=17
 uint8 MAXVEHAXLES_CHOICE=18
 uint8 MINVEHOCC_CHOICE=19
+uint8 MAXPLATOONSIZE_CHOICE=20
+uint8 MINPLATOONHDWY_CHOICE=21
 
 # signal ::= OCTET STRING SIZE(0..63)
 uint8[] signal
@@ -142,32 +146,35 @@ uint8 PARALLEL=1
 
 uint8 ANGLED=2
 
-#minspeed ::= INTEGER (0..1023), -- tenths of m/s
+#minspeed ::= INTEGER (0..1023), -- m/s
 float64 minspeed
 
-#maxspeed ::= INTEGER (0..1023), -- tenths of m/s
+#maxspeed ::= INTEGER (0..1023), -- m/s
 float64 maxspeed
 
-#minhdwy ::= INTEGER (0..2047), -- tenths of meters
+#minhdwy ::= INTEGER (0..2047), -- meters
 float64 minhdwy
 
 #maxvehmass ::= INTEGER (0..65535), -- kg
 float64 maxvehmass
 
-#maxvehheight ::= INTEGER (0..127), -- tenths of meters
+#maxvehheight ::= INTEGER (0..127), -- meters
 float64 maxvehheight
 
-#maxvehwidth ::= INTEGER (0..127), -- tenths of meters
+#maxvehwidth ::= INTEGER (0..127), -- meters
 float64 maxvehwidth
 
-#maxvehlength ::= INTEGER (0..1023), -- tenths of meters
+#maxvehlength ::= INTEGER (0..1023), -- meters
 float64 maxvehlength
 
-#maxvehaxles ::= INTEGER (2..15)
+#maxvehaxles ::= INTEGER (2..15), -- number of axles
 uint8 maxvehaxles
 
-#minvehocc ::= INTEGER (1..15)
+#minvehocc ::= INTEGER (1..15), -- vehicle passenger count
 uint8 minvehocc
 
+#maxplatoonsize ::= INTEGER (1..63), -- vehicle count
+uint8 maxplatoonsize
 
-
+#minplatoonhdwy ::= INTEGER (0..2047), -- meters
+float64 minplatoonhdwy
diff --git a/cav_msgs/msg/MobilityHeader.msg b/cav_msgs/msg/MobilityHeader.msg
index 370cbd42..b60705db 100644
--- a/cav_msgs/msg/MobilityHeader.msg
+++ b/cav_msgs/msg/MobilityHeader.msg
@@ -24,4 +24,3 @@ string  plan_id
 # A UNIX timestamp in milliseconds, which indicates the time elapsed since UNIX epoch
 # Example: "9223372036854775807"
 uint64  timestamp
-
diff --git a/cav_msgs/msg/MobilityOperation.msg b/cav_msgs/msg/MobilityOperation.msg
index ec8563e2..453e130d 100644
--- a/cav_msgs/msg/MobilityOperation.msg
+++ b/cav_msgs/msg/MobilityOperation.msg
@@ -12,4 +12,3 @@ string                   strategy
 # strategy parameters specified by each individual plugin/component
 # the maximum length of this string is 100
 string                   strategy_params
-
diff --git a/cav_msgs/msg/MobilityReason.msg b/cav_msgs/msg/MobilityReason.msg
new file mode 100644
index 00000000..a7f3cccf
--- /dev/null
+++ b/cav_msgs/msg/MobilityReason.msg
@@ -0,0 +1,18 @@
+# MobilityReason.msg
+#
+#
+
+# Variable which can be used to store a specific reason value.
+uint8 reason
+
+# enumeration values for mobility reason:
+
+uint8       UNKNOWN=0 
+uint8       ACCEPTED=1 
+uint8       SAFETY_VIOLATION=2 
+uint8       INSUFFICIENT_TIME=3
+uint8       PLAN_CONFLICT=4
+uint8       OTHERWISE_ENGAGED=5
+uint8       INSUFFICIENT_URGENCY=6
+uint8       PLAN_UNCLEAR=7
+uint8       OTHER=8
\ No newline at end of file
diff --git a/cav_msgs/msg/MobilityRepeat.msg b/cav_msgs/msg/MobilityRepeat.msg
new file mode 100644
index 00000000..3fbca201
--- /dev/null
+++ b/cav_msgs/msg/MobilityRepeat.msg
@@ -0,0 +1,12 @@
+# MobilityRepeat.msg
+#
+#
+
+# Variable which can be used to store a specific repeat value.
+uint8 repeat
+
+# enumeration values for mobility repeat:
+
+uint8       UNKNOWN=0 
+uint8       REQUEST_AGAIN=1 
+uint8       NEVER_REQUEST_AGAIN=2 
diff --git a/cav_msgs/msg/MobilityResponse.msg b/cav_msgs/msg/MobilityResponse.msg
index d9fd3e33..ce09ad96 100644
--- a/cav_msgs/msg/MobilityResponse.msg
+++ b/cav_msgs/msg/MobilityResponse.msg
@@ -4,10 +4,19 @@
 # recevied a MobilityRequest from that CAV and decide to involve in that plan
 
 # standard header for all mobility messages
-cav_msgs/MobilityHeader  m_header
+cav_msgs/MobilityHeader   m_header
 
 # urgency of the current plan from 0 to 1000
-uint16                   urgency
+uint16                    urgency
 
 # the content of this response
-bool                     is_accepted
+bool                      is_accepted
+
+# type of the proposal being suggested to neighbors
+cav_msgs/PlanType plan_type
+
+# 
+cav_msgs/MobilityReason   reason         # OPTIONAL
+
+# Whether or not to repeat the MobilityRequest
+cav_msgs/MobilityRepeat   repeat         # OPTIONAL
diff --git a/cav_msgs/msg/PlanType.msg b/cav_msgs/msg/PlanType.msg
index dfbcc574..e2f114f0 100644
--- a/cav_msgs/msg/PlanType.msg
+++ b/cav_msgs/msg/PlanType.msg
@@ -12,6 +12,16 @@ uint8       CHANGE_LANE_LEFT = 1
 uint8       CHANGE_LANE_RIGHT = 2
 uint8       JOIN_PLATOON_AT_REAR = 3
 uint8       PLATOON_FOLLOWER_JOIN = 4
+uint8       JOIN_PLATOON_FROM_FRONT = 5
+uint8       PLATOON_FRONT_JOIN = 6
+uint8       CUT_IN_FROM_SIDE = 7
+uint8       PLATOON_CUT_IN_JOIN = 8
+uint8       STOP_CREATE_GAP = 9
+uint8       CUT_IN_FRONT_DONE = 10
+uint8       CUT_IN_MID_OR_REAR_DONE = 11
+uint8       PLATOON_DEPARTURE = 12
+uint8       DELETE_MEMBER = 13
+
 
 # ideas for future additions:
 # uint8       SEARCHING_FOR_PLATOON = 5
@@ -21,3 +31,6 @@ uint8       PLATOON_FOLLOWER_JOIN = 4
 # uint8       PASSING = 9 
 # pulling onto shoulder, need multi-lane change
 # join platoon in middle, merging platoons
+# MobilityPlanType.msg
+#
+#
diff --git a/cav_msgs/msg/PlatooningInfo.msg b/cav_msgs/msg/PlatooningInfo.msg
index 8249365b..ae6aeb61 100644
--- a/cav_msgs/msg/PlatooningInfo.msg
+++ b/cav_msgs/msg/PlatooningInfo.msg
@@ -44,4 +44,19 @@ float32  host_cmd_speed
 float32  desired_gap
 
 # Actual gap with leader
-float32 actual_gap
\ No newline at end of file
+float32 actual_gap
+
+# The time headway summation of all predecessors   
+float32  current_predecessor_time_headway_sum
+
+# The speed of the preceding vehicle, in m/s.
+float32  predecessor_speed
+
+# The position of the preceding vehicle, in m.
+float32  predecessor_position
+
+# The status of gap creation.
+bool is_create_gap
+
+# Ratio to increase desired gap
+float32 create_gap_adjuster
diff --git a/cav_msgs/msg/RouteState.msg b/cav_msgs/msg/RouteState.msg
index 7c9c7410..3c334853 100644
--- a/cav_msgs/msg/RouteState.msg
+++ b/cav_msgs/msg/RouteState.msg
@@ -13,7 +13,7 @@
 Header header
 
 # The route id for which this message applies
-string routeID
+string route_id
 
 # Enum denoting the state of the route following
 uint8 state
diff --git a/cav_msgs/msg/TrafficControlDetail.msg b/cav_msgs/msg/TrafficControlDetail.msg
index 565189d2..5146656c 100644
--- a/cav_msgs/msg/TrafficControlDetail.msg
+++ b/cav_msgs/msg/TrafficControlDetail.msg
@@ -24,15 +24,17 @@
 #	lataffinity ENUMERATED {left, right},
 #	latperm SEQUENCE (SIZE(2)) OF ENUMERATED {none, permitted, passing-only, emergency-only},
 #	parking ENUMERATED {no, parallel, angled},
-#	minspeed INTEGER (0..1023), -- tenths of m/s
-#	maxspeed INTEGER (0..1023), -- tenths of m/s
-#	minhdwy INTEGER (0..2047), -- tenths of meters
-#	maxvehmass INTEGER (0..65535), -- kg
-#	maxvehheight INTEGER (0..127), -- tenths of meters
-#	maxvehwidth INTEGER (0..127), -- tenths of meters
-#	maxvehlength INTEGER (0..1023), -- tenths of meters
-#	maxvehaxles INTEGER (2..15),
-#	minvehocc INTEGER (1..15), 
+#	minspeed DOUBLE (0..1023), -- m/s
+#	maxspeed DOUBLE (0..1023), -- m/s
+#	minhdwy DOUBLE (0..2047), -- meters
+#	maxvehmass DOUBLE (0..65535), -- kg
+#	maxvehheight DOUBLE (0..127), -- meters
+#	maxvehwidth DOUBLE (0..127), -- meters
+#	maxvehlength DOUBLE (0..1023), -- meters
+#	maxvehaxles INTEGER (2..15), -- number of axles
+#	minvehocc INTEGER (1..15), -- vehicle passenger count
+#   maxplatoonsize INTEGER (1..63), -- vehicle count
+#   minplatoonhdwy DOUBLE (0..2047), -- meters
 #	...
 #}
 
@@ -61,6 +63,8 @@ uint8 MAXVEHWIDTH_CHOICE=16
 uint8 MAXVEHLENGTH_CHOICE=17
 uint8 MAXVEHAXLES_CHOICE=18
 uint8 MINVEHOCC_CHOICE=19
+uint8 MAXPLATOONSIZE_CHOICE=20
+uint8 MINPLATOONHDWY_CHOICE=21
 
 # signal ::= OCTET STRING SIZE(0..63)
 uint8[] signal
@@ -142,32 +146,35 @@ uint8 PARALLEL=1
 
 uint8 ANGLED=2
 
-#minspeed ::= INTEGER (0..1023), -- tenths of m/s
+#minspeed ::= INTEGER (0..1023), -- m/s
 float64 minspeed
 
-#maxspeed ::= INTEGER (0..1023), -- tenths of m/s
+#maxspeed ::= INTEGER (0..1023), -- m/s
 float64 maxspeed
 
-#minhdwy ::= INTEGER (0..2047), -- tenths of meters
+#minhdwy ::= INTEGER (0..2047), -- meters
 float64 minhdwy
 
 #maxvehmass ::= INTEGER (0..65535), -- kg
 float64 maxvehmass
 
-#maxvehheight ::= INTEGER (0..127), -- tenths of meters
+#maxvehheight ::= INTEGER (0..127), -- meters
 float64 maxvehheight
 
-#maxvehwidth ::= INTEGER (0..127), -- tenths of meters
+#maxvehwidth ::= INTEGER (0..127), -- meters
 float64 maxvehwidth
 
-#maxvehlength ::= INTEGER (0..1023), -- tenths of meters
+#maxvehlength ::= INTEGER (0..1023), -- meters
 float64 maxvehlength
 
-#maxvehaxles ::= INTEGER (2..15)
+#maxvehaxles ::= INTEGER (2..15), -- number of axles
 uint8 maxvehaxles
 
-#minvehocc ::= INTEGER (1..15)
+#minvehocc ::= INTEGER (1..15), -- vehicle passenger count
 uint8 minvehocc
 
+#maxplatoonsize ::= INTEGER (1..63), -- vehicle count
+uint8 maxplatoonsize
 
-
+#minplatoonhdwy ::= INTEGER (0..2047), -- meters
+float64 minplatoonhdwy
diff --git a/cav_srvs/srv/GetAvailableRoutes.srv b/cav_srvs/srv/GetAvailableRoutes.srv
index a62b5a1b..f3c75243 100644
--- a/cav_srvs/srv/GetAvailableRoutes.srv
+++ b/cav_srvs/srv/GetAvailableRoutes.srv
@@ -14,5 +14,5 @@
 
 # A list of available routes
 # For efficiency the routes provided in this list contain only an empty list segments
-cav_msgs/Route[] availableRoutes
+cav_msgs/Route[] available_routes
 
diff --git a/cav_srvs/srv/SetActiveRoute.srv b/cav_srvs/srv/SetActiveRoute.srv
index 41f8a665..548fb537 100644
--- a/cav_srvs/srv/SetActiveRoute.srv
+++ b/cav_srvs/srv/SetActiveRoute.srv
@@ -17,7 +17,7 @@ uint8 ROUTE_ID=0
 uint8 DESTINATION_POINTS_ARRAY=1
 
 # The id of the route to select
-string routeID
+string route_id
 
 # The array of destination points to be used for route generation 
 cav_msgs/Position3D[] destination_points
@@ -25,7 +25,7 @@ cav_msgs/Position3D[] destination_points
 ---
 # Response
 # An enumeration representing a service execution error as defined in the route design document.
-uint8 errorStatus
+uint8 error_status
 
 # enumeration values for errorStatus:
 uint8 NO_ERROR=0
diff --git a/docker/build-image.sh b/docker/build-image.sh
index f281fc55..668acdf8 100755
--- a/docker/build-image.sh
+++ b/docker/build-image.sh
@@ -44,9 +44,59 @@ while [[ $# -gt 0 ]]; do
             COMPONENT_VERSION_STRING=develop
             shift
             ;;
+        --ros-1-packages|--ros1)
+            ROS1_PACKAGES=""
+            shift
+            ;;
+        --ros-2-packages|--ros2)
+            ROS2_PACKAGES=""
+            shift
+            ;;
+        *)
+            # Var test based on Stack Overflow question: https://stackoverflow.com/questions/5406858/difference-between-unset-and-empty-variables-in-bash
+            # Asker: green69
+            # Answerer: geekosaur
+            if [ "${ROS2_PACKAGES+set}" = "set" ]; then
+                ROS2_PACKAGES="$ROS2_PACKAGES $arg"
+            elif [ "${ROS1_PACKAGES+set}" = "set" ]; then
+                ROS1_PACKAGES="$ROS1_PACKAGES $arg"
+            else
+                echo "Unknown argument $arg..."
+                exit -1
+            fi 
+            shift
+            ;;
     esac
 done
 
+if [[ ! -z "$ROS1_PACKAGES$ROS2_PACKAGES" ]]; then
+    echo "Performing incremental build of image to rebuild packages: $ROS1_PACKAGES $ROS2_PACKAGES..."
+
+    echo "Updating Dockerfile references to use most recent image as base image"
+    # Trim of docker image LS command sourced from 
+    # https://stackoverflow.com/questions/50625619/why-doesnt-the-cut-command-work-for-a-docker-image-ls-command
+    # Question Asker: Chris F
+    # Question Answerer: Arount
+    MOST_RECENT_IMAGE_DATA=$(docker image ls | grep $IMAGE | tr -s ' ')
+
+    if [[ -z "$MOST_RECENT_IMAGE_DATA" ]]; then
+        echo No prior image exists to use as base, an initial image must be built first before attempting incremental build.
+        exit -1
+    fi
+
+    MOST_RECENT_IMAGE_HASH=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 3)
+    MOST_RECENT_IMAGE_ORG=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 1 | cut -d "/" -f 1)
+    MOST_RECENT_IMAGE_TAG=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 2)
+    MOST_RECENT_IMAGE_DATE=$(echo $MOST_RECENT_IMAGE_DATA | cut -d " " -f 4,5,6)
+
+    echo Using $MOST_RECENT_IMAGE_TAG $MOST_RECENT_IMAGE_HASH $MOST_RECENT_IMAGE_DATE as base for partial build...
+
+    sed -i "s|^FROM[[:space:]]*[^[:space:]]*|FROM $MOST_RECENT_IMAGE_HASH|I" ../Dockerfile
+
+    COMPONENT_VERSION_STRING="SNAPSHOT"
+    USERNAME="local"
+fi
+
 if [[ -z "$COMPONENT_VERSION_STRING" ]]; then
     COMPONENT_VERSION_STRING=$("./get-component-version.sh")
 fi
@@ -57,7 +107,14 @@ echo "Final image name: $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING"
 cd ..
 if [[ $COMPONENT_VERSION_STRING = "develop" ]]; then
     sed "s|usdotfhwastoldev/|$USERNAME/|g; s|usdotfhwastolcandidate/|$USERNAME/|g; s|usdotfhwastol/|$USERNAME/|g; s|:[0-9]*\.[0-9]*\.[0-9]*|:$COMPONENT_VERSION_STRING|g; s|checkout.bash|checkout.bash -d|g" \
-        Dockerfile | docker build -f - --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \
+        Dockerfile | docker build --network=host -f - --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \
+        --build-arg VERSION="$COMPONENT_VERSION_STRING" \
+        --build-arg VCS_REF=`git rev-parse --short HEAD` \
+        --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` .
+elif [[ $COMPONENT_VERSION_STRING = "SNAPSHOT" ]]; then
+    docker build --network=host --no-cache -t $USERNAME/$IMAGE:$COMPONENT_VERSION_STRING \
+        --build-arg ROS1_PACKAGES="$ROS1_PACKAGES" \
+        --build-arg ROS2_PACKAGES="$ROS2_PACKAGES" \
         --build-arg VERSION="$COMPONENT_VERSION_STRING" \
         --build-arg VCS_REF=`git rev-parse --short HEAD` \
         --build-arg BUILD_DATE=`date -u +”%Y-%m-%dT%H:%M:%SZ”` .
diff --git a/docker/checkout.bash b/docker/checkout.bash
new file mode 100755
index 00000000..e37019ed
--- /dev/null
+++ b/docker/checkout.bash
@@ -0,0 +1,53 @@
+#!/bin/bash
+
+#  Copyright (C) 2018-2021 LEIDOS.
+# 
+#  Licensed under the Apache License, Version 2.0 (the "License"); you may not
+#  use this file except in compliance with the License. You may obtain a copy of
+#  the License at
+# 
+#  http://www.apache.org/licenses/LICENSE-2.0
+# 
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+#  WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+#  License for the specific language governing permissions and limitations under
+#  the License.
+
+# CARMA packages checkout script
+# Optional argument to set the root checkout directory with no ending '/' default is '~'
+
+set -exo pipefail
+
+dir=~
+while [[ $# -gt 0 ]]; do
+      arg="$1"
+      case $arg in
+            -d|--develop)
+                  BRANCH=develop
+                  shift
+            ;;
+            -r|--root)
+                  dir=$2
+                  shift
+                  shift
+            ;;
+      esac
+done
+
+cd ${dir}/src
+
+
+# clone carma repos
+
+if [[ "$BRANCH" = "develop" ]]; then
+      cd /home/carma/
+      git clone https://github.com/usdot-fhwa-stol/autoware.ai.git --depth 1 --branch carma-develop
+      cd ${dir}/src
+      git clone --depth 1 --branch develop  https://github.com/usdot-fhwa-stol/ros1_bridge.git
+else
+      cd /home/carma/
+      git clone https://github.com/usdot-fhwa-stol/autoware.ai.git --depth 1 --branch carma-system-4.2.0
+      cd ~/.base-image/workspace/src
+      git clone --depth 1 --branch carma-system-4.2.0 https://github.com/usdot-fhwa-stol/ros1_bridge.git
+fi
diff --git a/docker/install.sh b/docker/install.sh
new file mode 100755
index 00000000..8fda6bf3
--- /dev/null
+++ b/docker/install.sh
@@ -0,0 +1,31 @@
+# ROS 1 msgs setup
+/home/carma/.base-image/ros1_msgs_ws/src/carma_msgs/docker/checkout.bash
+cp -R /home/carma/autoware.ai/messages /home/carma/.base-image/ros1_msgs_ws/src/autoware.ai/
+cp -R /home/carma/autoware.ai/jsk_common_msgs /home/carma/.base-image/ros1_msgs_ws/src/autoware.ai/
+cp -R /home/carma/autoware.ai/jsk_recognition /home/carma/.base-image/ros1_msgs_ws/src/autoware.ai/
+
+# ROS 2 msgs setup
+mkdir -p ~/.base-image/ros2_msgs_ws/src/autoware.ai
+cp -R /home/carma/autoware.ai/messages /home/carma/.base-image/ros2_msgs_ws/src/autoware.ai/
+cp -R /home/carma/autoware.ai/jsk_common_msgs /home/carma/.base-image/ros2_msgs_ws/src/autoware.ai/
+cp -R /home/carma/autoware.ai/jsk_recognition /home/carma/.base-image/ros2_msgs_ws/src/autoware.ai/
+
+# Cleanup autoware repo once messages have been moved
+rm -r /home/carma/autoware.ai/
+
+# ROS1 message setup
+cd ~/.base-image/ros1_msgs_ws && source /opt/ros/noetic/setup.bash && colcon build --packages-up-to autoware_msgs cav_msgs cav_srvs j2735_msgs can_msgs carma_debug_msgs autoware_lanelet2_msgs
+
+# ROS2 message setup
+cd ~/.base-image/ros2_msgs_ws && source /opt/ros/foxy/setup.bash && colcon build --packages-up-to autoware_msgs autoware_lanelet2_msgs can_msgs carma_debug_ros2_msgs carma_driver_msgs carma_localization_msgs carma_msgs carma_perception_msgs carma_planning_msgs carma_v2x_msgs j2735_v2x_msgs
+
+# Build the bridge
+source /opt/ros/noetic/setup.bash 
+source /opt/ros/foxy/setup.bash 
+source ~/.base-image/ros1_msgs_ws/install/local_setup.bash 
+source ~/.base-image/ros2_msgs_ws/install/local_setup.bash 
+cd ~/.base-image/workspace/src 
+cd ../ 
+sudo apt-get update 
+colcon build --packages-select ros1_bridge --cmake-args "--debug-output"
+sudo chmod -R ugo+x ~/.base-image/workspace/install
diff --git a/j2735_msgs/msg/TrafficControlDetail.msg b/j2735_msgs/msg/TrafficControlDetail.msg
index 714df0d4..b3ba00a3 100644
--- a/j2735_msgs/msg/TrafficControlDetail.msg
+++ b/j2735_msgs/msg/TrafficControlDetail.msg
@@ -31,8 +31,10 @@
 #	maxvehheight INTEGER (0..127), -- tenths of meters
 #	maxvehwidth INTEGER (0..127), -- tenths of meters
 #	maxvehlength INTEGER (0..1023), -- tenths of meters
-#	maxvehaxles INTEGER (2..15),
-#	minvehocc INTEGER (1..15), 
+#	maxvehaxles INTEGER (2..15), -- number of axles
+#	minvehocc INTEGER (1..15), -- vehicle passenger count
+#   maxplatoonsize INTEGER (1..63), -- vehicle count
+#   minplatoonhdwy INTEGER (0..2047), -- tenths of meters
 #	...
 #}
 
@@ -61,6 +63,8 @@ uint8 MAXVEHWIDTH_CHOICE =16
 uint8 MAXVEHLENGTH_CHOICE =17
 uint8 MAXVEHAXLES_CHOICE =18
 uint8 MINVEHOCC_CHOICE =19
+uint8 MAXPLATOONSIZE_CHOICE =20
+uint8 MINPLATOONHDWY_CHOICE =21
 
 # signal ::= OCTET STRING SIZE(0..63)
 uint8[] signal
@@ -163,12 +167,14 @@ uint8 maxvehwidth
 #maxvehlength ::= INTEGER (0..1023), -- tenths of meters
 uint16 maxvehlength
 
-#maxvehaxles ::= INTEGER (2..15)
+#maxvehaxles ::= INTEGER (2..15), -- number of axles
 uint8 maxvehaxles
 
-#minvehocc ::= INTEGER (1..15)
+#minvehocc ::= INTEGER (1..15), -- vehicle passenger count
 uint8 minvehocc
 
+#maxplatoonsize ::= INTEGER (1..63), -- vehicle count
+uint8 maxplatoonsize
 
-
-
+#minplatoonhdwy ::= INTEGER (0..2047), -- tenths of meters
+uint16 minplatoonhdwy
diff --git a/j2735_v2x_msgs/msg/TrafficControlDetail.msg b/j2735_v2x_msgs/msg/TrafficControlDetail.msg
index 714df0d4..b3ba00a3 100644
--- a/j2735_v2x_msgs/msg/TrafficControlDetail.msg
+++ b/j2735_v2x_msgs/msg/TrafficControlDetail.msg
@@ -31,8 +31,10 @@
 #	maxvehheight INTEGER (0..127), -- tenths of meters
 #	maxvehwidth INTEGER (0..127), -- tenths of meters
 #	maxvehlength INTEGER (0..1023), -- tenths of meters
-#	maxvehaxles INTEGER (2..15),
-#	minvehocc INTEGER (1..15), 
+#	maxvehaxles INTEGER (2..15), -- number of axles
+#	minvehocc INTEGER (1..15), -- vehicle passenger count
+#   maxplatoonsize INTEGER (1..63), -- vehicle count
+#   minplatoonhdwy INTEGER (0..2047), -- tenths of meters
 #	...
 #}
 
@@ -61,6 +63,8 @@ uint8 MAXVEHWIDTH_CHOICE =16
 uint8 MAXVEHLENGTH_CHOICE =17
 uint8 MAXVEHAXLES_CHOICE =18
 uint8 MINVEHOCC_CHOICE =19
+uint8 MAXPLATOONSIZE_CHOICE =20
+uint8 MINPLATOONHDWY_CHOICE =21
 
 # signal ::= OCTET STRING SIZE(0..63)
 uint8[] signal
@@ -163,12 +167,14 @@ uint8 maxvehwidth
 #maxvehlength ::= INTEGER (0..1023), -- tenths of meters
 uint16 maxvehlength
 
-#maxvehaxles ::= INTEGER (2..15)
+#maxvehaxles ::= INTEGER (2..15), -- number of axles
 uint8 maxvehaxles
 
-#minvehocc ::= INTEGER (1..15)
+#minvehocc ::= INTEGER (1..15), -- vehicle passenger count
 uint8 minvehocc
 
+#maxplatoonsize ::= INTEGER (1..63), -- vehicle count
+uint8 maxplatoonsize
 
-
-
+#minplatoonhdwy ::= INTEGER (0..2047), -- tenths of meters
+uint16 minplatoonhdwy