Skip to content

Commit c7688d5

Browse files
evan-palmerevan-palmer
committed
Added support for loading as a component
1 parent eef43e4 commit c7688d5

File tree

5 files changed

+70
-48
lines changed

5 files changed

+70
-48
lines changed

‎libwaterlinked/CMakeLists.txt

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,6 @@ install(
5656
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
5757
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
5858
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
59-
INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
6059
FILE_SET HEADERS
6160
)
6261

‎waterlinked_dvl_driver/CMakeLists.txt

Lines changed: 34 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -15,35 +15,56 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
1515
marine_acoustic_msgs
1616
nav_msgs
1717
tf2_geometry_msgs
18+
rclcpp_components
19+
libwaterlinked
1820
)
1921

22+
foreach(pkg IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
23+
find_package(${pkg} REQUIRED)
24+
endforeach()
25+
2026
find_package(ament_cmake REQUIRED)
2127
find_package(generate_parameter_library REQUIRED)
22-
find_package(libwaterlinked REQUIRED)
23-
24-
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
25-
find_package(${Dependency} REQUIRED)
26-
endforeach()
2728

2829
generate_parameter_library(waterlinked_dvl_driver_parameters
2930
src/waterlinked_dvl_driver_parameters.yaml
3031
)
3132

32-
add_executable(waterlinked_dvl_driver)
33-
target_sources(waterlinked_dvl_driver PRIVATE src/waterlinked_dvl_driver.cpp)
33+
add_library(waterlinked_dvl_driver_library SHARED)
3434

35-
target_compile_features(waterlinked_dvl_driver PUBLIC cxx_std_20)
35+
target_sources(
36+
waterlinked_dvl_driver_library
37+
PRIVATE src/waterlinked_dvl_driver.cpp
38+
PUBLIC
39+
FILE_SET HEADERS
40+
BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
41+
FILES
42+
${CMAKE_CURRENT_SOURCE_DIR}/include/waterlinked_dvl_driver/waterlinked_dvl_driver.hpp
43+
)
3644
target_link_libraries(
37-
waterlinked_dvl_driver
38-
PRIVATE waterlinked_dvl_driver_parameters libwaterlinked::libwaterlinked
45+
waterlinked_dvl_driver_library
46+
PRIVATE waterlinked_dvl_driver_parameters
47+
)
48+
ament_target_dependencies(waterlinked_dvl_driver_library PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
49+
target_compile_features(waterlinked_dvl_driver_library PUBLIC cxx_std_20)
50+
51+
rclcpp_components_register_node(waterlinked_dvl_driver_library
52+
PLUGIN "waterlinked::ros::WaterLinkedDvlDriver"
53+
EXECUTABLE waterlinked_dvl_driver
3954
)
40-
ament_target_dependencies(waterlinked_dvl_driver PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
4155

4256
install(
43-
TARGETS waterlinked_dvl_driver waterlinked_dvl_driver_parameters
44-
RUNTIME DESTINATION ${CMAKE_INSTALL_LIBDIR}/waterlinked_dvl_driver
57+
TARGETS waterlinked_dvl_driver_library waterlinked_dvl_driver_parameters
58+
EXPORT export_waterlinked_dvl_driver_library
59+
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
60+
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
61+
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
62+
FILE_SET HEADERS
4563
)
4664

4765
install(DIRECTORY config launch DESTINATION share/waterlinked_dvl_driver)
4866

67+
ament_export_targets(export_waterlinked_dvl_driver_library HAS_LIBRARY_TARGET)
68+
ament_export_dependencies($THIS_PACKAGE_INCLUDE_DEPENDS)
69+
4970
ament_package()

‎waterlinked_dvl_driver/src/waterlinked_dvl_driver.hpp renamed to ‎waterlinked_dvl_driver/include/waterlinked_dvl_driver/waterlinked_dvl_driver.hpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@
2626
#include "libwaterlinked/client.hpp"
2727
#include "marine_acoustic_msgs/msg/dvl.hpp"
2828
#include "nav_msgs/msg/odometry.hpp"
29-
// #include "rclcpp_components/register_node_macro.hpp"
29+
#include "rclcpp_components/register_node_macro.hpp"
3030
#include "rclcpp_lifecycle/lifecycle_node.hpp"
3131
#include "std_srvs/srv/set_bool.hpp"
3232
#include "std_srvs/srv/trigger.hpp"
@@ -42,7 +42,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
4242
class WaterLinkedDvlDriver : public rclcpp_lifecycle::LifecycleNode
4343
{
4444
public:
45-
WaterLinkedDvlDriver();
45+
WaterLinkedDvlDriver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
4646

4747
auto on_configure(const rclcpp_lifecycle::State & previous_state) -> CallbackReturn override;
4848

@@ -74,4 +74,4 @@ class WaterLinkedDvlDriver : public rclcpp_lifecycle::LifecycleNode
7474

7575
} // namespace waterlinked::ros
7676

77-
// RCLCPP_COMPONENTS_REGISTER_NODE(waterlinked::driver::WaterLinkedDvlDriver)
77+
RCLCPP_COMPONENTS_REGISTER_NODE(waterlinked::ros::WaterLinkedDvlDriver)

‎waterlinked_dvl_driver/package.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
<depend>nav_msgs</depend>
2222
<depend>tf2_geometry_msgs</depend>
2323
<depend>std_srvs</depend>
24+
<depend>rclcpp_components</depend>
2425

2526
<test_depend>ament_lint_auto</test_depend>
2627
<test_depend>ament_lint_common</test_depend>

‎waterlinked_dvl_driver/src/waterlinked_dvl_driver.cpp

Lines changed: 32 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
1919
// SOFTWARE.
2020

21-
#include "waterlinked_dvl_driver.hpp"
21+
#include "waterlinked_dvl_driver/waterlinked_dvl_driver.hpp"
2222

2323
#include "rclcpp/rclcpp.hpp"
2424
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -49,36 +49,9 @@ auto populate_service_response(
4949

5050
} // namespace
5151

52-
WaterLinkedDvlDriver::WaterLinkedDvlDriver()
53-
: rclcpp_lifecycle::LifecycleNode("waterlinked_dvl_driver")
52+
WaterLinkedDvlDriver::WaterLinkedDvlDriver(const rclcpp::NodeOptions & options)
53+
: rclcpp_lifecycle::LifecycleNode("waterlinked_dvl_driver", options)
5454
{
55-
dvl_msg_.velocity_mode = marine_acoustic_msgs::msg::Dvl::DVL_MODE_BOTTOM;
56-
dvl_msg_.dvl_type = marine_acoustic_msgs::msg::Dvl::DVL_TYPE_PISTON; // 4-beam convex Janus array
57-
58-
// The following has been retrieved from:
59-
// https://github.com/ndahn/dvl_a50/blob/1e6a5304235facf53ecf82043fb5ba4c8569016b/src/dvl_a50_ros2.cpp#L45
60-
61-
// Each beam points 22.5° away from the center, LED pointing forward.
62-
// Transducers are rotated 45° around Z.
63-
// Beam 1 (+135° from X)
64-
dvl_msg_.beam_unit_vec[0].x = -0.6532814824381883;
65-
dvl_msg_.beam_unit_vec[0].y = 0.6532814824381883;
66-
dvl_msg_.beam_unit_vec[0].z = 0.38268343236508984;
67-
68-
// Beam 2 (-135° from X)
69-
dvl_msg_.beam_unit_vec[1].x = -0.6532814824381883;
70-
dvl_msg_.beam_unit_vec[1].y = -0.6532814824381883;
71-
dvl_msg_.beam_unit_vec[1].z = 0.38268343236508984;
72-
73-
// Beam 3 (-45° from X)
74-
dvl_msg_.beam_unit_vec[2].x = 0.6532814824381883;
75-
dvl_msg_.beam_unit_vec[2].y = -0.6532814824381883;
76-
dvl_msg_.beam_unit_vec[2].z = 0.38268343236508984;
77-
78-
// Beam 4 (+45° from X)
79-
dvl_msg_.beam_unit_vec[3].x = 0.6532814824381883;
80-
dvl_msg_.beam_unit_vec[3].y = 0.6532814824381883;
81-
dvl_msg_.beam_unit_vec[3].z = 0.38268343236508984;
8255
}
8356

8457
auto WaterLinkedDvlDriver::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) -> CallbackReturn
@@ -94,7 +67,6 @@ auto WaterLinkedDvlDriver::on_configure(const rclcpp_lifecycle::State & /*previo
9467
return CallbackReturn::ERROR;
9568
}
9669

97-
// The client does 99% of the work, this is just a ROS wrapper around its functionality
9870
try {
9971
client_ =
10072
std::make_unique<WaterLinkedClient>(params_.ip_address, params_.port, std::chrono::seconds(params_.timeout));
@@ -122,10 +94,39 @@ auto WaterLinkedDvlDriver::on_configure(const rclcpp_lifecycle::State & /*previo
12294
return CallbackReturn::ERROR;
12395
}
12496

97+
// Pre-populate the sensor state messages with known, static values
12598
dvl_msg_.header.frame_id = params_.frame_id;
12699
dead_reckoning_msg_.header.frame_id = params_.frame_id;
127100
odom_msg_.header.frame_id = params_.frame_id;
128101

102+
dvl_msg_.velocity_mode = marine_acoustic_msgs::msg::Dvl::DVL_MODE_BOTTOM;
103+
dvl_msg_.dvl_type = marine_acoustic_msgs::msg::Dvl::DVL_TYPE_PISTON; // 4-beam convex Janus array
104+
105+
// The following has been retrieved from:
106+
// https://github.com/ndahn/dvl_a50/blob/1e6a5304235facf53ecf82043fb5ba4c8569016b/src/dvl_a50_ros2.cpp#L45
107+
108+
// Each beam points 22.5° away from the center, LED pointing forward.
109+
// Transducers are rotated 45° around Z.
110+
// Beam 1 (+135° from X)
111+
dvl_msg_.beam_unit_vec[0].x = -0.6532814824381883;
112+
dvl_msg_.beam_unit_vec[0].y = 0.6532814824381883;
113+
dvl_msg_.beam_unit_vec[0].z = 0.38268343236508984;
114+
115+
// Beam 2 (-135° from X)
116+
dvl_msg_.beam_unit_vec[1].x = -0.6532814824381883;
117+
dvl_msg_.beam_unit_vec[1].y = -0.6532814824381883;
118+
dvl_msg_.beam_unit_vec[1].z = 0.38268343236508984;
119+
120+
// Beam 3 (-45° from X)
121+
dvl_msg_.beam_unit_vec[2].x = 0.6532814824381883;
122+
dvl_msg_.beam_unit_vec[2].y = -0.6532814824381883;
123+
dvl_msg_.beam_unit_vec[2].z = 0.38268343236508984;
124+
125+
// Beam 4 (+45° from X)
126+
dvl_msg_.beam_unit_vec[3].x = 0.6532814824381883;
127+
dvl_msg_.beam_unit_vec[3].y = 0.6532814824381883;
128+
dvl_msg_.beam_unit_vec[3].z = 0.38268343236508984;
129+
129130
dvl_pub_ = create_publisher<marine_acoustic_msgs::msg::Dvl>("~/velocity_report", rclcpp::SystemDefaultsQoS());
130131
odom_pub_ = create_publisher<nav_msgs::msg::Odometry>("~/odom", rclcpp::SystemDefaultsQoS());
131132
dead_reckoning_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(

0 commit comments

Comments
 (0)