From de7bfe63955835cac2c58a8991b4b3a73a4d0e90 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 20 Feb 2024 15:28:54 -0500 Subject: [PATCH 1/3] Fix lie naming and add comments --- .../tag_detector/tag_detector.processing.cpp | 2 +- src/util/lie/lie.cpp | 18 +++++----- src/util/lie/lie.hpp | 35 ++++++++++++++----- 3 files changed, 37 insertions(+), 18 deletions(-) diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index 7676206d2..bf4026da8 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -108,7 +108,7 @@ namespace mrover { std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); // Publish tag to odom std::string const& parentFrameId = mUseOdom ? mOdomFrameId : mMapFrameId; - SE3d tagInParent = SE3Conversions::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); + SE3d tagInParent = SE3Conversions::fromTfTree(mTfBuffer, immediateFrameId, parentFrameId); SE3Conversions::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); } catch (tf2::ExtrapolationException const&) { NODELET_WARN("Old data for immediate tag"); diff --git a/src/util/lie/lie.cpp b/src/util/lie/lie.cpp index b58beee26..664e03820 100644 --- a/src/util/lie/lie.cpp +++ b/src/util/lie/lie.cpp @@ -12,13 +12,13 @@ auto SIM3::position() const -> R3 { return mTransform.translation(); } -auto SE3Conversions::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) -> SE3d { - geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); +auto SE3Conversions::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrame, std::string const& toFrame) -> SE3d { + geometry_msgs::TransformStamped transform = buffer.lookupTransform(toFrame, fromFrame, ros::Time{}); return fromTf(transform.transform); } -auto SE3Conversions::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) -> void { - broadcaster.sendTransform(toTransformStamped(tf, parentFrameId, childFrameId)); +auto SE3Conversions::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& fromFrame, std::string const& toFrame, SE3d const& transform) -> void { + broadcaster.sendTransform(toTransformStamped(transform, fromFrame, toFrame)); } auto SE3Conversions::fromTf(geometry_msgs::Transform const& transform) -> SE3d { @@ -57,19 +57,19 @@ auto SE3Conversions::toTransform(SE3d const& tf) -> geometry_msgs::Transform { return transform; } -auto SE3Conversions::toPoseStamped(SE3d const& tf, std::string const& frameId) -> geometry_msgs::PoseStamped { +auto SE3Conversions::toPoseStamped(SE3d const& tf, std::string const& frame) -> geometry_msgs::PoseStamped { geometry_msgs::PoseStamped pose; pose.pose = toPose(tf); - pose.header.frame_id = frameId; + pose.header.frame_id = frame; pose.header.stamp = ros::Time::now(); return pose; } -auto SE3Conversions::toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) -> geometry_msgs::TransformStamped { +auto SE3Conversions::toTransformStamped(SE3d const& tf, std::string const& childFrame, std::string const& parentFrame) -> geometry_msgs::TransformStamped { geometry_msgs::TransformStamped transform; transform.transform = toTransform(tf); - transform.header.frame_id = parentFrameId; + transform.header.frame_id = parentFrame; transform.header.stamp = ros::Time::now(); - transform.child_frame_id = childFrameId; + transform.child_frame_id = childFrame; return transform; } diff --git a/src/util/lie/lie.hpp b/src/util/lie/lie.hpp index 1e51e4dab..59ec1d2b8 100644 --- a/src/util/lie/lie.hpp +++ b/src/util/lie/lie.hpp @@ -10,7 +10,7 @@ #include #include -using manif::SE3d, manif::SO3d; +using manif::SE3d, manif::SO3d, manif::SE3f, manif::SO3f; using R3 = Eigen::Vector3d; using S3 = Eigen::Quaterniond; @@ -25,13 +25,32 @@ class SE3Conversions { [[nodiscard]] static auto toTransform(SE3d const& tf) -> geometry_msgs::Transform; - [[nodiscard]] static auto toPoseStamped(SE3d const& tf, std::string const& frameId) -> geometry_msgs::PoseStamped; - - [[nodiscard]] static auto toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) -> geometry_msgs::TransformStamped; - - [[nodiscard]] static auto fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) -> SE3d; - - static auto pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) -> void; + [[nodiscard]] static auto toPoseStamped(SE3d const& tf, std::string const& frame) -> geometry_msgs::PoseStamped; + + [[nodiscard]] static auto toTransformStamped(SE3d const& tf, std::string const& childFrame, std::string const& parentFrame) -> geometry_msgs::TransformStamped; + + /** + * \brief Pull the most recent transform or pose between two frames from the TF tree. + * The second and third parameters are named for the transform interpretation. + * Consider them named "a" and "b" respectively: + * For a transform this is a rotation and translation, i.e. aToB. + * For a pose this is a position and orientation, i.e. aInB. + * \param buffer ROS TF Buffer, make sure a listener is attached + * \param fromFrame From (transform) or child (pose) frame + * \param toFrame To (transform) or parent (pose) frame + * \return The transform or pose represented by an SE3 lie group element + */ + [[nodiscard]] static auto fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrame, std::string const& toFrame) -> SE3d; + + /** + * \brief Push a transform to the TF tree between two frames + * \see fromTfTree for more explanation of the frames + * \param broadcaster ROS TF Broadcaster + * \param fromFrame From (transform) or child (pose) frame + * \param toFrame To (transform) or parent (pose) frame + * \param transform The transform or pose represented by an SE3 lie group element + */ + static auto pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& fromFrame, std::string const& toFrame, SE3d const& transform) -> void; }; class SIM3 { From 1d21ff7471bab528c574ffa32e62cb7c81e37354 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 20 Feb 2024 15:29:33 -0500 Subject: [PATCH 2/3] Check nav state in integration, make auton launch file not launch tensorrt obj detection by default --- launch/autonomy.launch | 2 ++ launch/jetson_autonomy.launch | 4 +++- launch/perception.launch | 10 +++------- launch/simulator.launch | 4 ++-- msg/NavMetadata.msg | 1 - msg/NavState.msg | 1 - test/integration/integration.py | 18 +++++++++++++----- test/integration/integration.test | 2 +- 8 files changed, 24 insertions(+), 18 deletions(-) delete mode 100644 msg/NavMetadata.msg delete mode 100644 msg/NavState.msg diff --git a/launch/autonomy.launch b/launch/autonomy.launch index 411a9e202..2312591c3 100644 --- a/launch/autonomy.launch +++ b/launch/autonomy.launch @@ -1,5 +1,6 @@ + @@ -7,6 +8,7 @@ + diff --git a/launch/jetson_autonomy.launch b/launch/jetson_autonomy.launch index ac64503af..7d9c3d0da 100644 --- a/launch/jetson_autonomy.launch +++ b/launch/jetson_autonomy.launch @@ -2,5 +2,7 @@ - + + + \ No newline at end of file diff --git a/launch/perception.launch b/launch/perception.launch index af742d42d..50e541223 100644 --- a/launch/perception.launch +++ b/launch/perception.launch @@ -1,15 +1,11 @@ - + - - + pkg="nodelet" type="nodelet" name="tag_detector" + args="load mrover/TagDetectorNodelet perception_nodelet_manager" output="screen" /> - + args="load mrover/SimulatorNodelet perception_nodelet_manager" output="screen"> diff --git a/msg/NavMetadata.msg b/msg/NavMetadata.msg deleted file mode 100644 index 722382710..000000000 --- a/msg/NavMetadata.msg +++ /dev/null @@ -1 +0,0 @@ -GPSWaypoint[] point \ No newline at end of file diff --git a/msg/NavState.msg b/msg/NavState.msg deleted file mode 100644 index 14e61b105..000000000 --- a/msg/NavState.msg +++ /dev/null @@ -1 +0,0 @@ -bool completed # TODO - temporary \ No newline at end of file diff --git a/test/integration/integration.py b/test/integration/integration.py index 8c3efc752..f2c5b51d2 100755 --- a/test/integration/integration.py +++ b/test/integration/integration.py @@ -2,13 +2,14 @@ import sys import unittest +from typing import Optional import numpy as np import rospy import rostest import tf2_ros -from mrover.msg import Waypoint, WaypointType, LED +from mrover.msg import Waypoint, WaypointType, StateMachineStateUpdate from util.SE3 import SE3 from util.course_publish_helpers import publish_waypoints, convert_waypoint_to_gps @@ -24,7 +25,7 @@ def test_integration(self): waypoints = [ ( Waypoint(tag_id=0, type=WaypointType(val=WaypointType.NO_SEARCH)), - SE3(position=np.array([4, 4, 0])), + SE3(position=np.array([5, 3, 0])), ), ( Waypoint(tag_id=0, type=WaypointType(val=WaypointType.POST)), @@ -41,6 +42,14 @@ def test_integration(self): # # rospy.Subscriber("/led", LED, led_callback) + nav_state = StateMachineStateUpdate() + + def nav_state_callback(msg: StateMachineStateUpdate) -> None: + nonlocal nav_state + nav_state = msg + + rospy.Subscriber("/nav_state", StateMachineStateUpdate, nav_state_callback) + rate = rospy.Rate(20) for waypoint in waypoints: waypoint_data, pose = waypoint @@ -60,16 +69,15 @@ def distance_to_target(): else: raise ValueError(f"Unknown waypoint type: {waypoint_data.type}") distance_to_target = waypoint_in_world.pos_distance_to(rover_in_world) - rospy.logdebug(distance_to_target) return distance_to_target except tf2_ros.TransformException as e: rospy.logwarn_throttle(1, f"Transform exception: {e}") return float("inf") - while distance_to_target() > COMPLETION_TOLERANCE: + while (distance := distance_to_target()) > COMPLETION_TOLERANCE and nav_state.state != "DoneState": if rospy.is_shutdown(): return - rospy.loginfo_throttle(1, f"Distance to current waypoint: {distance_to_target()}") + rospy.loginfo_throttle(1, f"Distance to current waypoint: {distance}, navigation state: {nav_state.state}") rate.sleep() rospy.loginfo("Waypoint reached") diff --git a/test/integration/integration.test b/test/integration/integration.test index 02c24f216..7c0186ae3 100644 --- a/test/integration/integration.test +++ b/test/integration/integration.test @@ -5,7 +5,7 @@ - + From 42b556b472916cba0fe307d3195637d00feaddcc Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 20 Feb 2024 15:56:19 -0500 Subject: [PATCH 3/3] Lower noise for now --- config/sim_ekf.yaml | 114 ++++++++++++++++++------------------ src/simulator/simulator.hpp | 12 ++-- 2 files changed, 63 insertions(+), 63 deletions(-) diff --git a/config/sim_ekf.yaml b/config/sim_ekf.yaml index 3d16bb477..9d2b75e3c 100644 --- a/config/sim_ekf.yaml +++ b/config/sim_ekf.yaml @@ -5,28 +5,28 @@ global_ekf: # This option controls whether to use that DoP covariance matrix, or to # replace it with the configured gps_covariane matrix below use_gps_dop_covariance: false - gps_covariance: [[2, 0, 0], - [0, 2, 0], - [0, 0, 2]] + gps_covariance: [ [ 0.02, 0, 0 ], + [ 0, 0.02, 0 ], + [ 0, 0, 0.02 ] ] - imu_orientation_covariance: [[0.1, 0, 0], - [0, 0.1, 0], - [0, 0, 0.1]] + imu_orientation_covariance: [ [ 0.01, 0, 0 ], + [ 0, 0.01, 0 ], + [ 0, 0, 0.01 ] ] - imu_accel_covariance: [[0.1, 0, 0], - [0, 0.1, 0], - [0, 0, 0.1]] + imu_accel_covariance: [ [ 0.01, 0, 0 ], + [ 0, 0.01, 0 ], + [ 0, 0, 0.01 ] ] - imu_gyro_covariance: [[0.1, 0, 0], - [0, 0.1, 0], - [0, 0, 0.1]] + imu_gyro_covariance: [ [ 0.01, 0, 0 ], + [ 0, 0.01, 0 ], + [ 0, 0, 0.01 ] ] - imu_mag_pose_covariance: [[0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0.1]] + imu_mag_pose_covariance: [ [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0.1 ] ] # Hz frequency: 30 @@ -69,11 +69,11 @@ global_ekf: # true, true, true, # false, true, true] - pose0_config: [true, true, true, - true, true, true, - false, false, false, - false, false, false, - false, false, false] + pose0_config: [ true, true, true, + true, true, true, + false, false, false, + false, false, false, + false, false, false ] # pose1_config: [false, false, false, # false, false, true, @@ -95,40 +95,40 @@ global_ekf: use_control: true # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. - control_config: [true, false, false, false, false, true] - acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] - deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] + control_config: [ true, false, false, false, false, true ] + acceleration_limits: [ 1.3, 0.0, 0.0, 0.0, 0.0, 3.4 ] + deceleration_limits: [ 1.3, 0.0, 0.0, 0.0, 0.0, 4.5 ] # x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az - process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] - - initial_estimate_covariance: [0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + process_noise_covariance: [ 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015 ] + + initial_estimate_covariance: [ 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9 ] diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index ea319a0fe..cf512bac9 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -227,13 +227,13 @@ namespace mrover { // TODO: make variances configurable std::default_random_engine mRNG; - std::normal_distribution mGPSDist{0, 0.2}, - mAccelDist{0, 0.05}, - mGyroDist{0, 0.02}, + std::normal_distribution<> mGPSDist{0, 0.02}, + mAccelDist{0, 0.01}, + mGyroDist{0, 0.01}, mMagDist{0, 0.1}, - mRollDist{0, 0.05}, - mPitchDist{0, 0.05}, - mYawDist{0, 0.1}; + mRollDist{0, 0.01}, + mPitchDist{0, 0.01}, + mYawDist{0, 0.01}; PeriodicTask mGpsTask; PeriodicTask mImuTask;