Skip to content

Commit f5c9fc0

Browse files
evan-palmerevan-palmer
committed
address final merge conflicts
1 parent 5227bf4 commit f5c9fc0

File tree

3 files changed

+49
-113
lines changed

3 files changed

+49
-113
lines changed

‎thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp‎

Lines changed: 1 addition & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -31,24 +31,6 @@
3131
namespace thruster_allocation_matrix_controller
3232
{
3333

34-
<<<<<<< HEAD
35-
namespace
36-
{
37-
38-
auto reset_wrench_msg(geometry_msgs::msg::Wrench * wrench_msg) -> void // NOLINT
39-
{
40-
wrench_msg->force.x = std::numeric_limits<double>::quiet_NaN();
41-
wrench_msg->force.y = std::numeric_limits<double>::quiet_NaN();
42-
wrench_msg->force.z = std::numeric_limits<double>::quiet_NaN();
43-
wrench_msg->torque.x = std::numeric_limits<double>::quiet_NaN();
44-
wrench_msg->torque.y = std::numeric_limits<double>::quiet_NaN();
45-
wrench_msg->torque.z = std::numeric_limits<double>::quiet_NaN();
46-
}
47-
48-
} // namespace
49-
50-
=======
51-
>>>>>>> origin/main
5234
auto ThrusterAllocationMatrixController::on_init() -> controller_interface::CallbackReturn
5335
{
5436
param_listener_ = std::make_shared<thruster_allocation_matrix_controller::ParamListener>(get_node());
@@ -120,12 +102,7 @@ auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::St
120102
}
121103

122104
reference_.writeFromNonRT(geometry_msgs::msg::Wrench());
123-
<<<<<<< HEAD
124-
125-
command_interfaces_.reserve(num_thrusters_);
126-
=======
127105
command_interfaces_.reserve(n_thrusters_);
128-
>>>>>>> origin/main
129106

130107
reference_sub_ = get_node()->create_subscription<geometry_msgs::msg::Wrench>(
131108
"~/reference",
@@ -150,11 +127,7 @@ auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::St
150127
auto ThrusterAllocationMatrixController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
151128
-> controller_interface::CallbackReturn
152129
{
153-
<<<<<<< HEAD
154-
reset_wrench_msg(reference_.readFromNonRT());
155-
=======
156130
common::messages::reset_message(reference_.readFromNonRT());
157-
>>>>>>> origin/main
158131
reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits<double>::quiet_NaN());
159132
return controller_interface::CallbackReturn::SUCCESS;
160133
}
@@ -208,33 +181,14 @@ auto ThrusterAllocationMatrixController::update_reference_from_subscribers(
208181
const rclcpp::Duration & /*period*/) -> controller_interface::return_type
209182
{
210183
auto * current_reference = reference_.readFromNonRT();
211-
<<<<<<< HEAD
212-
213-
const std::vector<double> wrench = {
214-
current_reference->force.x,
215-
current_reference->force.y,
216-
current_reference->force.z,
217-
current_reference->torque.x,
218-
current_reference->torque.y,
219-
current_reference->torque.z};
220-
221-
for (std::size_t i = 0; i < wrench.size(); ++i) {
222-
if (!std::isnan(wrench[i])) {
223-
reference_interfaces_[i] = wrench[i];
224-
}
225-
}
226-
227-
reset_wrench_msg(current_reference);
228-
229-
=======
230184
const std::vector<double> wrench = common::messages::to_vector(*current_reference);
231185
for (auto && [interface, ref] : std::views::zip(reference_interfaces_, wrench)) {
232186
if (!std::isnan(ref)) {
233187
interface = ref;
234188
}
235189
}
190+
236191
common::messages::reset_message(current_reference);
237-
>>>>>>> origin/main
238192
return controller_interface::return_type::OK;
239193
}
240194

‎thruster_controllers/src/polynomial_thrust_curve_controller.cpp‎

Lines changed: 0 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -71,16 +71,7 @@ auto PolynomialThrustCurveController::configure_parameters() -> controller_inter
7171
auto PolynomialThrustCurveController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
7272
-> controller_interface::CallbackReturn
7373
{
74-
<<<<<<< HEAD
75-
auto ret = configure_parameters();
76-
if (ret != controller_interface::CallbackReturn::SUCCESS) {
77-
return ret;
78-
}
79-
80-
reference_.writeFromNonRT(std_msgs::msg::Float64());
81-
=======
8274
configure_parameters();
83-
>>>>>>> origin/main
8475

8576
reference_.writeFromNonRT(std_msgs::msg::Float64());
8677
command_interfaces_.reserve(1);
@@ -147,10 +138,6 @@ auto PolynomialThrustCurveController::update_reference_from_subscribers(
147138
auto * current_reference = reference_.readFromNonRT();
148139
reference_interfaces_[0] = current_reference->data;
149140
current_reference->data = std::numeric_limits<double>::quiet_NaN();
150-
<<<<<<< HEAD
151-
152-
=======
153-
>>>>>>> origin/main
154141
return controller_interface::return_type::OK;
155142
}
156143

‎velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp‎

Lines changed: 48 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -31,11 +31,6 @@
3131
#include "controller_interface/chainable_controller_interface.hpp"
3232
#include "controller_interface/controller_interface.hpp"
3333
#include "geometry_msgs/msg/twist_stamped.hpp"
34-
<<<<<<< HEAD
35-
#include "hydrodynamics/eigen.hpp"
36-
== == ==
37-
=
38-
>>>>>>> origin/main
3934
#include "hydrodynamics/hydrodynamics.hpp"
4035
#include "nav_msgs/msg/odometry.hpp"
4136
#include "rclcpp/rclcpp.hpp"
@@ -50,77 +45,77 @@
5045
// auto-generated by generate_parameter_library
5146
#include <velocity_controllers/integral_sliding_mode_controller_parameters.hpp>
5247

53-
namespace velocity_controllers
48+
namespace velocity_controllers
5449
{
55-
class IntegralSlidingModeController : public controller_interface::ChainableControllerInterface
56-
{
57-
public:
58-
IntegralSlidingModeController() = default;
50+
class IntegralSlidingModeController : public controller_interface::ChainableControllerInterface
51+
{
52+
public:
53+
IntegralSlidingModeController() = default;
5954

60-
auto on_init() -> controller_interface::CallbackReturn override;
55+
auto on_init() -> controller_interface::CallbackReturn override;
6156

62-
auto on_configure(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override;
57+
auto on_configure(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override;
6358

64-
auto on_activate(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override;
59+
auto on_activate(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override;
6560

66-
auto command_interface_configuration() const -> controller_interface::InterfaceConfiguration override;
61+
auto command_interface_configuration() const -> controller_interface::InterfaceConfiguration override;
6762

68-
auto state_interface_configuration() const -> controller_interface::InterfaceConfiguration override;
63+
auto state_interface_configuration() const -> controller_interface::InterfaceConfiguration override;
6964

70-
auto update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period)
71-
-> controller_interface::return_type override;
65+
auto update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period)
66+
-> controller_interface::return_type override;
7267

73-
protected:
74-
auto on_export_reference_interfaces() -> std::vector<hardware_interface::CommandInterface> override;
68+
protected:
69+
auto on_export_reference_interfaces() -> std::vector<hardware_interface::CommandInterface> override;
7570

76-
auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period)
77-
-> controller_interface::return_type override;
71+
auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period)
72+
-> controller_interface::return_type override;
7873

79-
auto update_system_state_values() -> controller_interface::return_type;
74+
auto update_system_state_values() -> controller_interface::return_type;
8075

81-
auto update_parameters() -> void;
76+
auto update_parameters() -> void;
8277

83-
auto configure_parameters() -> controller_interface::CallbackReturn;
78+
auto configure_parameters() -> controller_interface::CallbackReturn;
8479

85-
auto update_and_validate_interfaces() -> controller_interface::return_type;
80+
auto update_and_validate_interfaces() -> controller_interface::return_type;
8681

87-
realtime_tools::RealtimeBuffer<geometry_msgs::msg::Twist> reference_;
88-
std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::Twist>> reference_sub_;
82+
realtime_tools::RealtimeBuffer<geometry_msgs::msg::Twist> reference_;
83+
std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::Twist>> reference_sub_;
8984

90-
realtime_tools::RealtimeBuffer<nav_msgs::msg::Odometry> system_state_;
91-
std::shared_ptr<rclcpp::Subscription<nav_msgs::msg::Odometry>> system_state_sub_;
92-
std::vector<double> system_state_values_;
85+
realtime_tools::RealtimeBuffer<nav_msgs::msg::Odometry> system_state_;
86+
std::shared_ptr<rclcpp::Subscription<nav_msgs::msg::Odometry>> system_state_sub_;
87+
std::vector<double> system_state_values_;
9388

94-
// we need the system rotation from the inertial frame to the vehicle frame for the hydrodynamic model
95-
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
96-
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
89+
// we need the system rotation from the inertial frame to the vehicle frame for the hydrodynamic model
90+
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
91+
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
9792

98-
std::string inertial_frame_id_, vehicle_frame_id_;
99-
realtime_tools::RealtimeBuffer<Eigen::Quaterniond> system_rotation_;
93+
std::string inertial_frame_id_, vehicle_frame_id_;
94+
realtime_tools::RealtimeBuffer<Eigen::Quaterniond> system_rotation_;
10095

101-
using ControllerState = control_msgs::msg::MultiDOFStateStamped;
102-
std::shared_ptr<rclcpp::Publisher<ControllerState>> controller_state_pub_;
103-
std::unique_ptr<realtime_tools::RealtimePublisher<ControllerState>> rt_controller_state_pub_;
104-
ControllerState controller_state_;
96+
using ControllerState = control_msgs::msg::MultiDOFStateStamped;
97+
std::shared_ptr<rclcpp::Publisher<ControllerState>> controller_state_pub_;
98+
std::unique_ptr<realtime_tools::RealtimePublisher<ControllerState>> rt_controller_state_pub_;
99+
ControllerState controller_state_;
105100

106-
std::shared_ptr<integral_sliding_mode_controller::ParamListener> param_listener_;
107-
integral_sliding_mode_controller::Params params_;
101+
std::shared_ptr<integral_sliding_mode_controller::ParamListener> param_listener_;
102+
integral_sliding_mode_controller::Params params_;
108103

109-
// the dofs will always be of order six, but we make it configurable to allow for renaming
110-
std::vector<std::string> dofs_;
111-
std::size_t n_dofs_;
104+
// the dofs will always be of order six, but we make it configurable to allow for renaming
105+
std::vector<std::string> dofs_;
106+
std::size_t n_dofs_;
112107

113-
// controller gains/parameters
114-
Eigen::Matrix6d kp_, rho_;
115-
double boundary_thickness_;
108+
// controller gains/parameters
109+
Eigen::Matrix6d kp_, rho_;
110+
double boundary_thickness_;
116111

117-
// controller state
118-
bool first_update_{true};
119-
Eigen::Vector6d init_error_, total_error_;
112+
// controller state
113+
bool first_update_{true};
114+
Eigen::Vector6d init_error_, total_error_;
120115

121-
std::unique_ptr<hydrodynamics::Params> model_;
116+
std::unique_ptr<hydrodynamics::Params> model_;
122117

123-
rclcpp::Logger logger_{rclcpp::get_logger("integral_sliding_mode_controller")};
124-
};
118+
rclcpp::Logger logger_{rclcpp::get_logger("integral_sliding_mode_controller")};
119+
};
125120

126121
} // namespace velocity_controllers

0 commit comments

Comments
 (0)