DONT MERGE THIS PR

README.md

@@ -7,6 +7,13 @@ git clone https://github.com/naughtyStark/nhttc_ros.git
 cd nhttc_ros
 git submodule init
 git submodule update --recursive
+cd nhttc_ros/NHTTC
+git submodule init
+git submodule update --recursive
+```
+please ensure that you have xterm installed! (used for debugging).
+```
+sudo apt install xterm
 ```
 rviz setup:
 start roscore
@@ -37,3 +44,18 @@ Running:
 ```
 roslaunch nhttc_ros multi_teleop.launch
 ```
+
+To run on real car:
+
+1. On lab computer, set its ROS_IP to its own IP address (use `ip a` to check and `export ROS_IP=XXX.XXX.XXX.XXX`). Then run roscore.
+
+2. On lab computer, Initialize mocap pose stream with `roslaunch mushr_mocap vrpn.launch` and `roslaunch mushr_mocap car_pose_publisher.launch`. There are some context dependent arguments that need to be set, which are the car names, so be mindful of that.
+
+3. SSH into the car (the rest of the steps are done ssh-ed into the car)
+
+4. Set the car's ROS_IP to its own IP, and also set its ROS_MASTER_URI to the IP of the lab computer.
+
+5. Rostopic list on the car to see if it is able to read the car_pose topics published from master, and try to rostopic echo them as well.
+
+6. If everything is connected correctly, run `roslaunch nhttc_ros real.launch` on the car.
+

nhttc_ros/launch/map_server.launch

@@ -0,0 +1,5 @@
+<launch>
+    <arg name="map" default="$(find nhttc_ros)/maps/sandbox.yaml"/>
+    <node pkg="map_server" name="map_server" type="map_server" args="$(arg map)" />
+    <param name="/map_file" value="$(arg map)" />
+</launch>

nhttc_ros/launch/multi_teleop.launch

@@ -1,13 +1,13 @@
 <launch>
     <!-- <node type="rviz" name="rviz" pkg="rviz" args="-d $(find package_name)/rviz/multi_teleop.rviz" /> -->
-    <arg name="car1_name" default="car1" />
-    <arg name="car2_name" default="car2" />
-    <arg name="car3_name" default="car3" />
-    <arg name="car4_name" default="car4" />
+    <arg name="car1_name" default="car30" />
+    <arg name="car2_name" default="car38" />
+    <arg name="car3_name" default="car_sim" />
+    <!-- <arg name="car4_name" default="car4" />
     <arg name="car5_name" default="car5" />
     <arg name="car6_name" default="car6" />
     <arg name="car7_name" default="car7" />
-    <arg name="car8_name" default="car8" />
+    <arg name="car8_name" default="car8" /> -->
 
     <!-- Set to 1 if you want to run the map_server -->
     <arg name="map_server" value = "1"/>
@@ -19,47 +19,52 @@
 
     <!-- Launch first car -->
     <group ns="$(arg car1_name)">
-         <include file="$(find mushr_sim)/launch/single_car.launch" >
+         <!-- <include file="$(find mushr_sim)/launch/single_car.launch" >
             <arg name="car_name" value="$(arg car1_name)"/>
             <arg name="racecar_version" value="racecar-uw-nano"/>
             <arg name="racecar_color" value="" />
-        </include>
-        <node pkg="nhttc_ros" type="nhttc_ros_node" name="controller" launch-prefix="xterm -e" output="screen">
-            <param name="car_name" value="car1"/>
+        </include> -->
+        <node pkg="nhttc_ros" type="nhttc_ros_node" name="controller" output="screen">
+            <param name="self_name" value="$(arg car1_name)"/>
+            <param name="other_name1" value="$(arg car2_name)"/>
             <param name="solver_time" value="20"/>
-            <!-- <param name="sim" value="0"/> -->
-            <param name="max_agents" value="4"/>
+            <param name="sim" value="0"/>
+            <param name="max_agents" value="2"/>
         </node>
     </group>
 
     <!-- Launch second car -->
     <group ns="$(arg car2_name)">
-        <include file="$(find mushr_sim)/launch/single_car.launch" >
+        <!-- <include file="$(find mushr_sim)/launch/single_car.launch" >
             <arg name="car_name" value="$(arg car2_name)"/>
             <arg name="racecar_version" value="racecar-uw-nano"/>
             <arg name="racecar_color" value="-green-black" />
 
-        </include>
-        <node pkg="nhttc_ros" type="nhttc_ros_node" name="controller">
-            <param name="car_name" value="car2"/>
+        </include> -->
+        <node pkg="nhttc_ros" type="nhttc_ros_node" name="controller" launch-prefix="xterm -e" output="screen">
+            <param name="self_name" value="$(arg car2_name)"/>
+            <param name="other_name1" value="$(arg car1_name)"/>
             <param name="solver_time" value="20"/>
+            <param name="sim" value="0"/>
+            <param name="max_agents" value="2"/>
         </node>
     </group>
-
-<!--     <group ns="$(arg car3_name)">
+<!--
+     <group ns="$(arg car3_name)">
         <include file="$(find mushr_sim)/launch/single_car.launch" >
             <arg name="car_name" value="$(arg car3_name)"/>
             <arg name="racecar_version" value="racecar-uw-nano"/>
             <arg name="racecar_color" value="-green-black" />
 
         </include>
-        <node pkg="nhttc_ros" type="nhttc_ros_node" name="controller3">
-            <param name="car_name" value="car3"/>
+        <node pkg="nhttc_ros" type="nhttc_ros_node" name="controller3" launch-prefix="xterm -e" output="screen">
+            <param name="car_name" value="$(arg car3_name)"/>
+            <param name="sim" value="1"/>
             <param name="solver_time" value="20"/>
         </node>
-    </group>
+    </group>-->
 
-    <group ns="$(arg car4_name)">
+<!--    <group ns="$(arg car4_name)">
         <include file="$(find mushr_sim)/launch/single_car.launch" >
             <arg name="car_name" value="$(arg car4_name)"/>
             <arg name="racecar_version" value="racecar-uw-nano"/>
@@ -120,7 +125,12 @@
         </node> -->
     <!-- </group> -->
 
-    <node pkg="nhttc_ros" type="nhttc_pose_init.py" name="initializer"/>
+    <node pkg="nhttc_ros" type="nhttc_pose_goal_init.py" name="initializer" output="screen">
+        <param name="car_count" value="2"/>
+        <param name="car1_name-sim" value="$(arg car1_name),0"/>
+        <param name="car2_name-sim" value="$(arg car2_name),0"/>
+        <!-- <param name="car3_name-sim" value="$(arg car3_name),1"/> -->
+    </node>
     <!-- <node pkg="nhttc_ros" type="nhttc_publisher.py" name="waypointer"/> -->
     <!-- Launch third car, fourth car, etc by following above template -->
 

nhttc_ros/launch/real.launch

@@ -0,0 +1,62 @@
+<launch>
+    <!-- <node type="rviz" name="rviz" pkg="rviz" args="-d $(find package_name)/rviz/multi_teleop.rviz" /> -->
+    <arg name="own_name" default="car30" />
+
+    <!-- Total number of cars (including self) -->
+    <arg name="car_count" default = "2"/>
+
+    <!-- Other car names (if car_count is 2, then nodes will read own_name, other_car1, then disregard the rest) -->
+    <arg name="other_car1" default = "car38"/>
+    <arg name="other_car2" default = "car3"/>
+    <arg name="other_car3" default = "car4"/>
+    <arg name="other_car4" default = "car5"/>
+    <arg name="other_car5" default = "car6"/>
+    <arg name="other_car6" default = "car7"/>
+    <arg name="other_car7" default = "car8"/>
+
+    <!-- Set to 1 if you want to run the map_server -->
+    <arg name="map_server" value = "1"/>
+
+    <!-- Launch  map server-->
+    <group if="$(arg map_server)">
+        <include file="$(find mushr_sim)/launch/map_server.launch" />
+	<!-- <include file="$(find nhttc_ros)/launch/map_server.launch" /> -->
+    </group>
+
+    <!-- Launch Teleop -->
+    <group>
+	<include file="$(find mushr_base)/launch/teleop.launch" >
+	        <arg name="car_name" value="$(arg own_name)" />
+	</include>
+    </group>
+
+    <!-- Launch Own Controller -->
+    <group ns="$(arg own_name)">
+        <node pkg="nhttc_ros" type="nhttc_ros_node" name="controller" output="screen">
+            <param name="car_count" value="$(arg car_count)"/>
+            <param name="self_name" value="$(arg own_name)"/>
+            <param name="other_name1" value="$(arg other_car1)"/>
+
+            <param name="solver_time" value="20"/>
+            <param name="sim" value="0"/>
+
+            <!-- Currently set this equal to car count but this may need to be fixed -->
+            <param name="max_agents" value="$(arg car_count)"/>
+        </node>
+    </group>
+
+    <node pkg="nhttc_ros" type="nhttc_pose_goal_init.py" name="initializer" output="screen">
+        <param name="car_count" value="$(arg car_count)"/>
+        <!-- Each arg is a tuple of (car_name,boolean) of whether it is a sim car or not -->
+        <param name="car1_name-sim" value="$(arg own_name),0"/>
+        <param name="car2_name-sim" value="$(arg other_car1),0"/>
+        <param name="car3_name-sim" value="$(arg other_car2),1"/>
+        <param name="car4_name-sim" value="$(arg other_car3),1"/>
+        <param name="car5_name-sim" value="$(arg other_car4),1"/>
+        <param name="car6_name-sim" value="$(arg other_car5),1"/>
+        <param name="car7_name-sim" value="$(arg other_car6),1"/>
+	<param name="car8_name-sim" value="$(arg other_car7),1"/>
+    </node>
+    <!-- <node pkg="nhttc_ros" type="nhttc_publisher.py" name="waypointer"/> -->
+
+</launch>

nhttc_ros/launch/real.launch~

@@ -0,0 +1,32 @@
+<launch>
+    <!-- <node type="rviz" name="rviz" pkg="rviz" args="-d $(find package_name)/rviz/multi_teleop.rviz" /> -->
+    <arg name="car_name" default="car30" />
+
+    <!-- Set to 1 if you want to run the map_server -->
+    <arg name="map_server" value = "1"/>
+
+    <!-- Launch  map server-->
+    <group if="$(arg map_server)">
+       <!--  <include file="$(find mushr_sim)/launch/map_server.launch" /> -->
+	<include file="$(find mushr_rhc_ros)/launch/map_server.launch" />
+    </group>
+
+
+    <!-- Launch Car Node -->
+    <group ns="$(arg car_name)">
+        <node pkg="nhttc_ros" type="nhttc_ros_node" name="controller" output="screen">
+            <param name="car_name" value="$(arg car_name)"/>
+            <param name="solver_time" value="20"/>
+            <param name="sim" value="0"/>
+            <param name="max_agents" value="4"/>
+        </node>
+    </group>
+
+    <node pkg="nhttc_ros" type="nhttc_pose_init.py" name="initializer">
+        <param name="car_count" value="1"/>
+        <param name="car1_name-sim" value="$(arg car_name),0"/>
+    </node>
+    <!-- <node pkg="nhttc_ros" type="nhttc_publisher.py" name="waypointer"/> -->
+    <!-- Launch third car, fourth car, etc by following above template -->
+
+</launch>

nhttc_ros/maps/gates.yaml

@@ -0,0 +1,7 @@
+image: gates.pgm
+resolution: 0.020000
+origin: [-38.120000, -25.320000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

nhttc_ros/maps/mit/basement_hallways_10cm.yaml

@@ -0,0 +1,7 @@
+image: basement_hallways_10cm.png
+resolution: 0.100000
+origin: [-30.000000, -30.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

nhttc_ros/maps/mit/basement_hallways_5cm.yml

@@ -0,0 +1,7 @@
+image: basement_hallways_5cm.png
+resolution: 0.050000
+origin: [-30.000000, -30.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

nhttc_ros/maps/mit/short-course-33.yml

@@ -0,0 +1,7 @@
+image: short-course-33.png
+resolution: 0.025000
+origin: [-11.50000, -5.000000, -0.090000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

nhttc_ros/maps/real-floor0.yaml

@@ -0,0 +1,7 @@
+image: real-floor0.pgm
+resolution: 0.020000
+origin: [-37.480000, -24.040000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

nhttc_ros/maps/real-floor4_corridor.yaml

@@ -0,0 +1,7 @@
+image: real-floor4_corridor.pgm
+resolution: 0.020000
+origin: [-62.440000, -45.160000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

nhttc_ros/maps/sandbox.yaml

@@ -0,0 +1,6 @@
+image: sandbox.png
+resolution: 0.1
+origin: [-50, -50, 0]
+occupied_thresh: 0.1
+free_thresh: 0.09
+negate: 0

nhttc_ros/maps/sieg_floor3.yaml

@@ -0,0 +1,7 @@
+image: sieg_floor3.pgm
+resolution: 0.020000
+origin: [-34.920000, -33.640000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+