diff --git a/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp b/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp index 832d2523..bdf7fb6a 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp @@ -98,6 +98,10 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * All plugins are required to have a constructor that accepts no arguments */ Unicycle2DIgnition(); + Unicycle2DIgnition(Unicycle2DIgnition const&) = delete; + Unicycle2DIgnition& operator=(Unicycle2DIgnition const&) = delete; + Unicycle2DIgnition(Unicycle2DIgnition&&) = delete; + Unicycle2DIgnition& operator=(Unicycle2DIgnition&&) = delete; /** * @brief Destructor @@ -140,15 +144,16 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseServiceCallback(rclcpp::Service::SharedPtr service, - std::shared_ptr, const fuse_msgs::srv::SetPose::Request::SharedPtr req); + bool setPoseServiceCallback(rclcpp::Service::SharedPtr const& service, + std::shared_ptr const&, + fuse_msgs::srv::SetPose::Request::SharedPtr const& req); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseDeprecatedServiceCallback(rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); + bool setPoseDeprecatedServiceCallback(rclcpp::Service::SharedPtr const& service, + std::shared_ptr const& request_id, + fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr const& req); protected: /** @@ -164,7 +169,8 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, yaw) */ - void process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, std::function post_process = nullptr); + void process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + std::function const& post_process = nullptr); /** * @brief Create and send a prior transaction based on the supplied pose diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 4b40dade..904fbe09 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -161,9 +161,9 @@ void Unicycle2DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCo } } -bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPose::Request::SharedPtr req) +bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr const& service, + std::shared_ptr const& request_id, + fuse_msgs::srv::SetPose::Request::SharedPtr const& req) { try { @@ -185,8 +185,9 @@ bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr service, std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) + rclcpp::Service::SharedPtr const& service, + std::shared_ptr const& request_id, + fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr const& req) { try { @@ -205,7 +206,7 @@ bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( } void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - std::function post_process) + std::function const& post_process) { // Verify we are in the correct state to process set pose requests if (!started_) @@ -219,10 +220,10 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta std::to_string(pose.pose.pose.position.x) + ", " + std::to_string(pose.pose.pose.position.y) + ")."); } - auto orientation_norm = std::sqrt(pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + - pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + - pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + - pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); + auto orientation_norm = std::sqrt((pose.pose.pose.orientation.x * pose.pose.pose.orientation.x) + + (pose.pose.pose.orientation.y * pose.pose.pose.orientation.y) + + (pose.pose.pose.orientation.z * pose.pose.pose.orientation.z) + + (pose.pose.pose.orientation.w * pose.pose.pose.orientation.w)); if (std::abs(orientation_norm - 1.0) > 1.0e-3) { throw std::invalid_argument(