Add autonomy integration rostest back with new simulator

ansible/roles/build/tasks/main.yml

@@ -89,8 +89,6 @@
       - ros-{{ ros_distro }}-rosbash
       - libbullet-dev
       - libglfw3-dev
-      - xorg-dev
-      - libx11-xcb-dev
       - libnl-3-dev
       - libnl-route-3-dev
 

launch/simulator.launch

@@ -1,22 +1,26 @@
 <launch>
-    <arg name="run_rviz" default="true" />
-    <rosparam command="load" file="$(find mrover)/config/simulator/simulator.yaml" />
+  <arg name="headless" default="false"/>
+  <arg name="run_rviz" default="true"/>
 
-    <include file="$(find mrover)/launch/autonomy.launch">
-        <arg name="sim" value="true" />
-        <arg name="ekf_start_delay" value="1" />
-    </include>
+  <rosparam command="load" file="$(find mrover)/config/simulator/simulator.yaml"/>
 
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" required="true"
-        args="manager" output="screen" />
+  <include file="$(find mrover)/launch/autonomy.launch">
+    <arg name="sim" value="true"/>
+    <arg name="ekf_start_delay" value="1"/>
+  </include>
 
-    <node pkg="nodelet" type="nodelet" name="simulator_nodelet"
-        args="load mrover/SimulatorNodelet nodelet_manager" output="screen" />
+  <node pkg="nodelet" type="nodelet" name="nodelet_manager" required="true"
+        args="manager" output="screen"/>
 
-    <node name="arm_controller" pkg="mrover" type="arm_controller" output="screen" />
+  <node pkg="nodelet" type="nodelet" name="simulator_nodelet"
+        args="load mrover/SimulatorNodelet nodelet_manager" output="screen">
+    <param name="headless" value="$(arg headless)"/>
+  </node>
 
-    <group if="$(arg run_rviz)">
-        <arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz" />
-        <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" />
-    </group>
-</launch>
+  <node name="arm_controller" pkg="mrover" type="arm_controller" output="screen"/>
+
+  <group if="$(arg run_rviz)">
+    <arg name="rvizconfig" default="$(find mrover)/config/rviz/auton_sim.rviz"/>
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)"/>
+  </group>
+</launch>

scripts/build_dawn.sh

@@ -18,5 +18,7 @@ CC=clang CXX=clang++ cmake \
     -D DAWN_ENABLE_INSTALL=ON \
     -D BUILD_SHARED_LIBS=ON \
     -D DAWN_USE_GLFW=OFF \
-    -D TINT_BUILD_CMD_TOOLS=OFF
+    -D TINT_BUILD_CMD_TOOLS=OFF \
+    -D TINT_BUILD_DOCS=OFF \
+    -D TINT_BUILD_TESTS=OFF
 cmake --build out/Release

src/navigation/approach_post.py

@@ -24,7 +24,7 @@ def on_loop(self, context) -> State:
         :param ud:  State machine user data
         :return:    Next state
         """
-        fid_pos = context.env.current_fid_pos()
+        fid_pos = context.env.current_tag_pos()
         if fid_pos is None:
             return search.SearchState()
 
@@ -38,7 +38,7 @@ def on_loop(self, context) -> State:
             )
             if arrived:
                 context.env.arrived_at_post = True
-                context.env.last_post_location = context.env.current_fid_pos(odom_override=False)
+                context.env.last_post_location = context.env.current_tag_pos(odom_override=False)
                 context.course.increment_waypoint()
                 return waypoint.WaypointState()
             context.rover.send_drive_command(cmd_vel)

src/navigation/context.py

@@ -55,50 +55,50 @@ def get_pose_with_time(self) -> Tuple[SE3, Time]:
 class Environment:
     """
     Context class to represent the rover's environment
-    Information such as locations of fiducials or obstacles
+    Information such as locations of tags or obstacles
     """
 
     ctx: Context
-    NO_FIDUCIAL: ClassVar[int] = -1
+    NO_TAG: ClassVar[int] = -1
     arrived_at_post: bool = False
     last_post_location: Optional[np.ndarray] = None
 
-    def get_fid_pos(self, fid_id: int, in_odom_frame: bool = True) -> Optional[np.ndarray]:
+    def get_tag_pos(self, tag_id: int, in_odom_frame: bool = True) -> Optional[np.ndarray]:
         """
-        Retrieves the pose of the given fiducial ID from the TF tree in the odom frame
+        Retrieves the pose of the given tag ID from the TF tree in the odom frame
         if in_odom_frame is True otherwise in the world frame
         if it exists and is more recent than TAG_EXPIRATION_TIME_SECONDS, otherwise returns None
         """
         try:
             parent_frame = self.ctx.odom_frame if in_odom_frame else self.ctx.world_frame
             fid_pose, time = SE3.from_tf_time(
-                self.ctx.tf_buffer, parent_frame=parent_frame, child_frame=f"fiducial{fid_id}"
+                self.ctx.tf_buffer, parent_frame=parent_frame, child_frame=f"fiducial{tag_id}"
             )
             now = rospy.Time.now()
             if now.to_sec() - time.to_sec() >= TAG_EXPIRATION_TIME_SECONDS:
-                print(f"TAG EXPIRED {fid_id}!")
+                rospy.logwarn(f"Tag expired: {tag_id}!")
                 return None
         except (
             tf2_ros.LookupException,
             tf2_ros.ConnectivityException,
             tf2_ros.ExtrapolationException,
-        ) as e:
+        ):
             return None
         return fid_pose.position
 
-    def current_fid_pos(self, odom_override: bool = True) -> Optional[np.ndarray]:
+    def current_tag_pos(self, odom_override: bool = True) -> Optional[np.ndarray]:
         """
-        Retrieves the position of the current fiducial (and we are looking for it)
+        Retrieves the position of the current tag (and we are looking for it)
         :param: odom_override if false will force it to be in the map frame
         """
         assert self.ctx.course
         in_odom = self.ctx.use_odom and odom_override
         current_waypoint = self.ctx.course.current_waypoint()
         if current_waypoint is None:
-            print("CURRENT WAYPOINT IS NONE")
+            rospy.logwarn("Current waypoint is empty!")
             return None
 
-        return self.get_fid_pos(current_waypoint.tag_id, in_odom)
+        return self.get_tag_pos(current_waypoint.tag_id, in_odom)
 
 
 @dataclass
@@ -128,8 +128,7 @@ def current_waypoint(self) -> Optional[Waypoint]:
         """
         Returns the currently active waypoint
 
-        :param ud:  State machine user data
-        :return:    Next waypoint to reach if we have an active course
+        :return: Next waypoint to reach if we have an active course
         """
         if self.course_data is None or self.waypoint_index >= len(self.course_data.waypoints):
             return None

src/navigation/failure_identification/failure_identification.py

@@ -69,7 +69,6 @@ def __init__(self):
             + wheel_velocity_variables
             + command_variables
         )
-        print(self.cols)
         self._df = pd.DataFrame(columns=self.cols)
         self.watchdog = WatchDog()
         self.path_name = None  # type: ignore
@@ -80,7 +79,7 @@ def write_to_csv(self):
         """
         if self.actively_collecting and self.data_collecting_mode:
             # append to csv if csv exists else write to csv
-            rospy.loginfo("writing to file")
+            rospy.loginfo("Writing to file")
             if self.path_name is None:
                 path = Path.cwd() / "failure_data"
                 path.mkdir(exist_ok=True)
@@ -91,7 +90,7 @@ def write_to_csv(self):
                 self.path_name = path
                 self._df.to_csv(path)
 
-                rospy.loginfo("===== failure data written to csv =====")
+                rospy.loginfo("===== Failure data written to CSV =====")
             else:
                 self._df.to_csv(self.path_name, mode="a", header=False)
 
@@ -163,7 +162,7 @@ def update(self, nav_status: StateMachineStateUpdate, drive_status: MotorsStatus
             cur_row[f"wheel_{wheel_num}_velocity"] = drive_status.joint_states.velocity[wheel_num]
 
         # update the data frame with the cur row
-        self._df = pd.concat([self._df, pd.DataFrame([cur_row])])
+        self._df = pd.concat([self._df, pd.DataFrame([cur_row])]) if self._df.size else pd.DataFrame([cur_row])
 
         if len(self._df) == DATAFRAME_MAX_SIZE:
             self.write_to_csv()

src/navigation/failure_identification/watchdog.py

@@ -1,5 +1,6 @@
 from typing import Tuple
 
+import rospy
 import numpy as np
 import pandas as pd
 
@@ -50,18 +51,18 @@ def check_angular_stuck(self, delta_time, delta_rot, dataframe) -> bool:
         )
         linear_are_all_zero = dataframe["cmd_vel_x"].eq(0).all()
         angular_are_non_zero = dataframe["cmd_vel_twist"].ne(0).all()
-        print(
-            f"turn all same sign: f{turn_are_all_same_sign}, linear_are_all_zero: f{linear_are_all_zero}, angular are non zero: f{angular_are_non_zero}"
+        rospy.logdebug(
+            f"Turn all same sign: f{turn_are_all_same_sign}, linear all zero: f{linear_are_all_zero}, angular all nonzero: f{angular_are_non_zero}"
         )
         if not turn_are_all_same_sign or not linear_are_all_zero:
-            print("not turning")
+            rospy.logdebug("Not turning")
             return False
         if not angular_are_non_zero:
-            print("not turning")
+            rospy.logdebug("Not turning")
             return False
         # check if the angular velocity is less than the threshold for the entire dataframe
         angular_velocity = delta_rot / delta_time
-        print(f"angular velocity: {angular_velocity}, thresh: {ANGULAR_THRESHOLD}")
+        rospy.logdebug(f"{angular_velocity=}, {ANGULAR_THRESHOLD=}")
         return abs(angular_velocity) < ANGULAR_THRESHOLD
 
     def check_linear_stuck(self, delta_time, delta_pos, dataframe) -> bool:
@@ -70,14 +71,14 @@ def check_linear_stuck(self, delta_time, delta_pos, dataframe) -> bool:
             dataframe["cmd_vel_x"].apply(np.sign).eq(np.sign(dataframe["cmd_vel_x"].iloc[0])).all()
         )
         linear_are_all_non_zero = dataframe["cmd_vel_x"].ne(0).all()
-        print(linear_are_all_same_sign, linear_are_all_non_zero)
+        rospy.logdebug(f"{linear_are_all_same_sign=}, {linear_are_all_non_zero=}")
         if not linear_are_all_same_sign or not linear_are_all_non_zero:
-            print("not driving straight")
+            rospy.logdebug("Not driving straight")
             return False
         # check if the average linear velocity is less than the threshold for the entire dataframe
         linear_velocity = delta_pos / delta_time
         linear_velocity = np.linalg.norm(linear_velocity)
-        print(linear_velocity, LINEAR_THRESHOLD)
+        rospy.logdebug(f"{linear_velocity=}, {LINEAR_THRESHOLD=}")
         return linear_velocity < LINEAR_THRESHOLD
 
     def is_stuck(self, dataframe: pd.DataFrame) -> bool:
@@ -88,15 +89,15 @@ def is_stuck(self, dataframe: pd.DataFrame) -> bool:
             start_rot, end_rot = self.get_start_end_rotations(dataframe_sliced)
             # get the start and end time
             start_time, end_time = self.get_start_end_time(dataframe_sliced)
-            print(start_time, end_time)
+            rospy.logdebug(f"{start_time=}, {end_time=}")
             # get the delta time and delta position
             delta_time = end_time - start_time
             delta_pos = end_pos - start_pos
             # use SO3 distance to find the delta rotation
             delta_rot = start_rot.rot_distance_to(end_rot)
             # check if the rover is stuck
             if self.check_angular_stuck(delta_time, delta_rot, dataframe_sliced) or self.check_linear_stuck(
-                delta_time, delta_pos, dataframe_sliced
+                    delta_time, delta_pos, dataframe_sliced
             ):
                 return True
         return False

src/navigation/post_backup.py

@@ -4,6 +4,7 @@
 from typing import Optional
 
 import numpy as np
+import rospy
 import tf2_ros
 from shapely.geometry import Point, LineString
 
@@ -49,7 +50,6 @@ def avoid_post_trajectory(rover_pose: SE3, post_pos: np.ndarray, waypoint_pos: n
 
         rover_pos = rover_pose.position
         rover_direction = rover_pose.rotation.direction_vector()
-        print(rover_pos, post_pos, waypoint_pos)
 
         # Converting to 2d arrays
         post_pos = post_pos[:2]
@@ -88,7 +88,6 @@ def avoid_post_trajectory(rover_pose: SE3, post_pos: np.ndarray, waypoint_pos: n
 
         # add a z coordinate of 0 to all the coords
         coords = np.hstack((coords, np.zeros((coords.shape[0], 1))))
-        print(coords)
         return AvoidPostTrajectory(coords)
 
 
@@ -128,7 +127,7 @@ def on_loop(self, context) -> State:
                 drive_back=drive_backwards,
             )
             if arrived:
-                print(f"ARRIVED AT POINT {point_index}")
+                rospy.loginfo(f"Arrived at point indexed: {point_index}")
                 if self.traj.increment_point():
                     self.traj = None
                     return waypoint.WaypointState()
@@ -141,8 +140,8 @@ def on_loop(self, context) -> State:
             context.rover.send_drive_command(cmd_vel)
             return self
         except (
-            tf2_ros.LookupException,
-            tf2_ros.ConnectivityException,
-            tf2_ros.ExtrapolationException,
+                tf2_ros.LookupException,
+                tf2_ros.ConnectivityException,
+                tf2_ros.ExtrapolationException,
         ):
             return self

src/navigation/search.py

@@ -134,6 +134,6 @@ def on_loop(self, context) -> State:
         )
         context.rover.send_drive_command(cmd_vel)
 
-        if context.env.current_fid_pos() is not None and context.course.look_for_post():
+        if context.env.current_tag_pos() is not None and context.course.look_for_post():
             return approach_post.ApproachPostState()
         return self

src/navigation/waypoint.py

@@ -35,7 +35,7 @@ def on_loop(self, context) -> State:
             return post_backup.PostBackupState()
 
         if context.course.look_for_post():
-            if context.env.current_fid_pos() is not None:
+            if context.env.current_tag_pos() is not None:
                 return approach_post.ApproachPostState()
 
         # Attempt to find the waypoint in the TF tree and drive to it

src/simulator/simulator.cpp

@@ -25,13 +25,28 @@ namespace mrover {
 
         mIkTargetPub = mNh.advertise<IK>("/arm_ik", 1);
 
-        initWindow();
+        mIsHeadless = mPnh.param<bool>("headless", false);
+        mEnablePhysics = mIsHeadless;
+        {
+            XmlRpc::XmlRpcValue gpsLinearization;
+            mNh.getParam("gps_linearization", gpsLinearization);
+            if (gpsLinearization.getType() != XmlRpc::XmlRpcValue::TypeStruct) throw std::invalid_argument{"GPS lineraization must be a struct. Did you rosparam load a localization config file properly?"};
+
+            mGpsLinerizationReferencePoint = {
+                    xmlRpcValueToTypeOrDefault<double>(gpsLinearization, "reference_point_latitude"),
+                    xmlRpcValueToTypeOrDefault<double>(gpsLinearization, "reference_point_longitude"),
+                    xmlRpcValueToTypeOrDefault<double>(gpsLinearization, "reference_point_altitude"),
+            };
+            mGpsLinerizationReferenceHeading = xmlRpcValueToTypeOrDefault<double>(gpsLinearization, "reference_heading");
+        }
+
+        if (!mIsHeadless) initWindow();
 
         initPhysics();
 
         initRender();
 
-        parseParams();
+        initUrdfsFromParams();
 
         twistCallback(boost::make_shared<geometry_msgs::Twist>());
 
@@ -58,18 +73,21 @@ namespace mrover {
             // Note(quintin):
             // Apple sucks and does not support polling on a non-main thread (which we are currently on)
             // Instead add to ROS's global callback queue which I think will be served by the main thread
+            if (!mIsHeadless) {
 #ifdef __APPLE__
-            struct PollGlfw : ros::CallbackInterface {
-                auto call() -> CallResult override {
-                    glfwPollEvents();
-                    return Success;
-                }
-            };
-            ros::getGlobalCallbackQueue()->addCallback(boost::make_shared<PollGlfw>());
+                struct PollGlfw : ros::CallbackInterface {
+                    auto call() -> CallResult override {
+                        glfwPollEvents();
+                        return Success;
+                    }
+                };
+                ros::getGlobalCallbackQueue()->addCallback(boost::make_shared<PollGlfw>());
 #else
-            glfwPollEvents();
+                glfwPollEvents();
 #endif
-            // glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED);
+            }
+            // Comments this out while debugging segfaults, otherwise it captures your cursor
+            glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED);
             mLoopProfiler.measureEvent("GLFW Events");
 
             userControls(dt);
@@ -109,4 +127,4 @@ namespace mrover {
         }
     }
 
-} // namespace mrover
+} // namespace mrover