Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/integration' into gui-cameras
Browse files Browse the repository at this point in the history
  • Loading branch information
@qhdwight
qhdwight committed Feb 20, 2024
2 parents 7da8141 + 42b556b commit 839c36b
Show file tree
Hide file tree
Showing 13 changed files with 124 additions and 99 deletions.
114 changes: 57 additions & 57 deletions config/sim_ekf.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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,
Expand All @@ -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 ]

2 changes: 2 additions & 0 deletions launch/autonomy.launch
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,14 @@
<launch>
<arg name="run_tag_detector" default="true" />
<arg name="run_object_detector" default="false" />

<arg name="sim" default="false" />
<arg name="use_ekf" default="true" />
<arg name="ekf_start_delay" default="0" />

<include file="$(find mrover)/launch/perception.launch">
<arg name="run_tag_detector" value="$(arg run_tag_detector)" />
<arg name="run_object_detector" value="$(arg run_object_detector)" />
</include>

<include file="$(find mrover)/launch/navigation.launch" />
Expand Down
4 changes: 3 additions & 1 deletion launch/jetson_autonomy.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,5 +2,7 @@
<include file="$(find mrover)/launch/jetson_base.launch" />
<include file="$(find mrover)/launch/esw_autonomy.launch" />
<include file="$(find mrover)/launch/zed.launch" />
<include file="$(find mrover)/launch/autonomy.launch" />
<include file="$(find mrover)/launch/autonomy.launch">
<arg name="run_object_detector" value="true" />
</include>
</launch>
10 changes: 3 additions & 7 deletions launch/perception.launch
View file
Original file line number Diff line number Diff line change
@@ -1,15 +1,11 @@
<launch>
<arg name="run_tag_detector" default="true" />
<arg name="run_object_detector" default="true" />
<arg name="run_object_detector" default="false" />

<!-- Nodelet to detect AR tags and publish them to the TF tree -->
<node if="$(arg run_tag_detector)"
pkg="nodelet" type="nodelet" name="zed_tag_detector"
args="load mrover/TagDetectorNodelet nodelet_manager" output="screen" />

<node if="$(arg run_tag_detector)"
pkg="nodelet" type="nodelet" name="long_range_tag_detector"
args="load mrover/LongRangeTagDetectorNodelet nodelet_manager" output="screen" />
pkg="nodelet" type="nodelet" name="tag_detector"
args="load mrover/TagDetectorNodelet perception_nodelet_manager" output="screen" />

<node if="$(arg run_object_detector)"
pkg="nodelet" type="nodelet" name="object_detector" respawn="true"
Expand Down
4 changes: 2 additions & 2 deletions launch/simulator.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,11 @@
<arg name="ekf_start_delay" value="1"/>
</include>

<node pkg="nodelet" type="nodelet" name="nodelet_manager" required="true"
<node pkg="nodelet" type="nodelet" name="perception_nodelet_manager" required="true"
args="manager" output="screen"/>

<node pkg="nodelet" type="nodelet" name="simulator_nodelet"
args="load mrover/SimulatorNodelet nodelet_manager" output="screen">
args="load mrover/SimulatorNodelet perception_nodelet_manager" output="screen">
<param name="headless" value="$(arg headless)"/>
</node>

Expand Down
1 change: 0 additions & 1 deletion msg/NavMetadata.msg
View file

This file was deleted.

1 change: 0 additions & 1 deletion msg/NavState.msg
View file

This file was deleted.

2 changes: 1 addition & 1 deletion src/perception/tag_detector/tag_detector.processing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
12 changes: 6 additions & 6 deletions src/simulator/simulator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,13 +227,13 @@ namespace mrover {

// TODO: make variances configurable
std::default_random_engine mRNG;
std::normal_distribution<double> 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;
Expand Down
18 changes: 9 additions & 9 deletions src/util/lie/lie.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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;
}
35 changes: 27 additions & 8 deletions src/util/lie/lie.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
#include <Eigen/Geometry>
#include <manif/SE3.h>

using manif::SE3d, manif::SO3d;
using manif::SE3d, manif::SO3d, manif::SE3f, manif::SO3f;

using R3 = Eigen::Vector3d;
using S3 = Eigen::Quaterniond;
Expand All @@ -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 {
Expand Down
Loading

0 comments on commit 839c36b

Please sign in to comment.