diff --git a/src/subjugator/gnc/subjugator_localization/launch/subjugator_localization.launch.py b/src/subjugator/gnc/subjugator_localization/launch/subjugator_localization.launch.py
index 1103cfb..09cfb87 100644
--- a/src/subjugator/gnc/subjugator_localization/launch/subjugator_localization.launch.py
+++ b/src/subjugator/gnc/subjugator_localization/launch/subjugator_localization.launch.py
@@ -32,7 +32,7 @@ def generate_launch_description():
False,
True,
True,
- False,
+ True,
True,
True,
True,
@@ -42,30 +42,30 @@ def generate_launch_description():
Parameter("imu0_relative", True),
Parameter("imu0_remove_gravitational_acceleration", True),
Parameter("imu0_queue_size", 10),
- # Parameter("odom0", "/dvl/odom"),
- # Parameter(
- # "odom0_config",
- # [
- # True,
- # True,
- # False,
- # False,
- # False,
- # True,
- # True,
- # False,
- # False,
- # False,
- # False,
- # True,
- # False,
- # False,
- # False,
- # ],
- # ),
- # Parameter("odom0_differential", True),
- # Parameter("odom0_relative", True),
- # Parameter("odom0_queue_size", 10),
+ Parameter("odom0", "/dvl/velocity"),
+ Parameter(
+ "odom0_config",
+ [
+ False,
+ False,
+ False,
+ False,
+ False,
+ False,
+ True,
+ True,
+ True,
+ False,
+ False,
+ False,
+ False,
+ False,
+ False,
+ ],
+ ),
+ Parameter("odom0_differential", False),
+ Parameter("odom0_relative", True),
+ Parameter("odom0_queue_size", 10),
# Parameter("pose0", "/depth/pose"),
# Parameter("pose0_config", [False, False, True,
# False, False, False,
@@ -77,6 +77,7 @@ def generate_launch_description():
# Parameter("pose0_queue_size", 10),
Parameter("print_diagnostics", True),
Parameter("publish_acceleration", True),
+ # Parameter("initial_state", [0.0] * 15),
]
import rich
diff --git a/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro b/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro
index 422806a..6c3d926 100644
--- a/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro
+++ b/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro
@@ -1,6 +1,5 @@
-
+
-
@@ -17,7 +16,7 @@
-
+
@@ -39,11 +38,9 @@
-
+
-
+
base_link
-2
-5
@@ -65,37 +62,21 @@
-10
-
-
-
-
+
+
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro b/src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro
index 96e32e1..22d2a7f 100644
--- a/src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro
+++ b/src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro
@@ -1,17 +1,13 @@
-
-
+
-
+
-
+
@@ -25,15 +21,13 @@
- true
+ true
true
-
+
-
+
0 0 0 0 0 0
1
8
@@ -97,13 +91,10 @@
-
-
-
- /dvl/velocity
-
-
-
-
\ No newline at end of file
+
+
+ /dvl/velocity
+
+
+
+
diff --git a/src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro b/src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro
index 69628f2..3e208ac 100644
--- a/src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro
+++ b/src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro
@@ -1,17 +1,13 @@
-
-
-
-
+
+
+
-
+
@@ -25,16 +21,17 @@
- true
+ true
true
-
+
true
${rate}
${namespace}/data_raw
+ imu_link
@@ -71,21 +68,14 @@
-
-
-
+
true
${rate}
${namespace}/mag
-
-
+
-
-
\ No newline at end of file
+
+
diff --git a/src/subjugator/simulation/subjugator_gazebo/src/DVLBridge.cc b/src/subjugator/simulation/subjugator_gazebo/src/DVLBridge.cc
index c6ed4b3..c26f3f4 100644
--- a/src/subjugator/simulation/subjugator_gazebo/src/DVLBridge.cc
+++ b/src/subjugator/simulation/subjugator_gazebo/src/DVLBridge.cc
@@ -14,18 +14,17 @@
* limitations under the License.
*/
-#include
+#include "DVLBridge.hh"
#include
#include
+#include
#include
-#include "gz/plugin/Register.hh"
-#include "DVLBridge.hh"
+#include "gz/plugin/Register.hh"
-GZ_ADD_PLUGIN(
- dave_ros_gz_plugins::DVLBridge, gz::sim::System, dave_ros_gz_plugins::DVLBridge::ISystemConfigure,
- dave_ros_gz_plugins::DVLBridge::ISystemPostUpdate)
+GZ_ADD_PLUGIN(dave_ros_gz_plugins::DVLBridge, gz::sim::System, dave_ros_gz_plugins::DVLBridge::ISystemConfigure,
+ dave_ros_gz_plugins::DVLBridge::ISystemPostUpdate)
namespace dave_ros_gz_plugins
{
@@ -39,11 +38,12 @@ struct DVLBridge::PrivateData
rclcpp::Publisher::SharedPtr dvl_pub;
};
-DVLBridge::DVLBridge() : dataPtr(std::make_unique()) {}
+DVLBridge::DVLBridge() : dataPtr(std::make_unique())
+{
+}
-void DVLBridge::Configure(
- const gz::sim::Entity & _entity, const std::shared_ptr & _sdf,
- gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager)
+void DVLBridge::Configure(gz::sim::Entity const &_entity, std::shared_ptr const &_sdf,
+ gz::sim::EntityComponentManager &_ecm, gz::sim::EventManager &_eventManager)
{
gzdbg << "dave_ros_gz_plugins::DVLBridge::Configure on entity: " << _entity << std::endl;
@@ -67,17 +67,16 @@ void DVLBridge::Configure(
}
// Gazebo subscriber
- std::function callback =
- std::bind(&DVLBridge::receiveGazeboCallback, this, std::placeholders::_1);
+ std::function callback =
+ std::bind(&DVLBridge::receiveGazeboCallback, this, std::placeholders::_1);
this->dataPtr->gz_node.Subscribe(this->dataPtr->dvl_topic, callback);
// ROS2 publisher
- this->dataPtr->dvl_pub =
- this->ros_node_->create_publisher(this->dataPtr->dvl_topic, 1);
+ this->dataPtr->dvl_pub = this->ros_node_->create_publisher(this->dataPtr->dvl_topic, 1);
}
-void DVLBridge::receiveGazeboCallback(const gz::msgs::DVLVelocityTracking & msg)
+void DVLBridge::receiveGazeboCallback(gz::msgs::DVLVelocityTracking const &msg)
{
std::lock_guard lock(this->dataPtr->mutex_);
@@ -86,15 +85,17 @@ void DVLBridge::receiveGazeboCallback(const gz::msgs::DVLVelocityTracking & msg)
auto dvl_msg = nav_msgs::msg::Odometry();
dvl_msg.header.stamp.sec = msg.header().stamp().sec();
dvl_msg.header.stamp.nanosec = msg.header().stamp().nsec();
+ dvl_msg.header.frame_id = "base_link";
+ dvl_msg.child_frame_id = "dvl_link";
dvl_msg.twist.twist.linear.x = msg.velocity().mean().x();
dvl_msg.twist.twist.linear.y = msg.velocity().mean().y();
dvl_msg.twist.twist.linear.z = msg.velocity().mean().z();
// Simulated DVL does not produce a pose (at least not one we can use)
- dvl_msg.pose.pose.position.x = -1;
- dvl_msg.pose.pose.position.y = -1;
- dvl_msg.pose.pose.position.z = -1;
+ dvl_msg.pose.pose.position.x = 0;
+ dvl_msg.pose.pose.position.y = 0;
+ dvl_msg.pose.pose.position.z = 0;
// Limit to 9 so the output is not bloated with empty covariance values
size_t covariance_size = msg.velocity().covariance().size();
@@ -107,8 +108,7 @@ void DVLBridge::receiveGazeboCallback(const gz::msgs::DVLVelocityTracking & msg)
this->dataPtr->dvl_pub->publish(dvl_msg);
}
-void DVLBridge::PostUpdate(
- const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm)
+void DVLBridge::PostUpdate(gz::sim::UpdateInfo const &_info, gz::sim::EntityComponentManager const &_ecm)
{
if (!_info.paused)
{
@@ -121,4 +121,4 @@ void DVLBridge::PostUpdate(
}
}
-} // namespace dave_ros_gz_plugins
\ No newline at end of file
+} // namespace dave_ros_gz_plugins