diff --git a/ff_estimate/CMakeLists.txt b/ff_estimate/CMakeLists.txt index acfbdaa..612b907 100644 --- a/ff_estimate/CMakeLists.txt +++ b/ff_estimate/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(rclcpp REQUIRED) find_package(ff_msgs REQUIRED) # Estimation library -add_library(est_lib src/base_estimator.cpp src/pose2d_estimator.cpp) +add_library(est_lib src/base_mocap_estimator.cpp) target_include_directories(est_lib PUBLIC $ $) @@ -38,10 +38,10 @@ install( ) # motion capture estimator node -add_executable(mocap_estimator_node src/mocap_estimator_node.cpp) -target_link_libraries(mocap_estimator_node est_lib) +add_executable(moving_avg_estimator_node src/moving_avg_estimator_node.cpp) +target_link_libraries(moving_avg_estimator_node est_lib) -install(TARGETS mocap_estimator_node +install(TARGETS moving_avg_estimator_node DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/ff_estimate/include/ff_estimate/base_estimator.hpp b/ff_estimate/include/ff_estimate/base_mocap_estimator.hpp similarity index 71% rename from ff_estimate/include/ff_estimate/base_estimator.hpp rename to ff_estimate/include/ff_estimate/base_mocap_estimator.hpp index 62585f5..5c0ed56 100644 --- a/ff_estimate/include/ff_estimate/base_estimator.hpp +++ b/ff_estimate/include/ff_estimate/base_mocap_estimator.hpp @@ -29,6 +29,8 @@ #include "ff_msgs/msg/free_flyer_state.hpp" #include "ff_msgs/msg/free_flyer_state_stamped.hpp" +#include "ff_msgs/msg/pose2_d_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" namespace ff { @@ -36,10 +38,15 @@ namespace ff /** * @brief estimator base class */ -class BaseEstimator : public rclcpp::Node +class BaseMocapEstimator : public rclcpp::Node { public: - explicit BaseEstimator(const std::string & node_name); + /** + * @brief constructor + * + * @param node_name ROS2 node name, default to mocap_estimator + */ + explicit BaseMocapEstimator(const std::string & node_name = "mocap_estimator"); protected: /** @@ -49,8 +56,18 @@ class BaseEstimator : public rclcpp::Node */ void SendStateEstimate(const ff_msgs::msg::FreeFlyerState & state); + /** + * @brief abstract estimator function + * + * @param pose_stamped time-stamped 2D pose measurement + */ + virtual void EstimateWithPose2D(const ff_msgs::msg::Pose2DStamped & pose_stamped) = 0; + private: rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Subscription::SharedPtr pose_sub_; + + void PoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr pose); }; } // namespace ff diff --git a/ff_estimate/include/ff_estimate/pose2d_estimator.hpp b/ff_estimate/include/ff_estimate/pose2d_estimator.hpp deleted file mode 100644 index 2b8077d..0000000 --- a/ff_estimate/include/ff_estimate/pose2d_estimator.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// MIT License -// -// Copyright (c) 2023 Stanford Autonomous Systems Lab -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - - -#pragma once - -#include - -#include "ff_estimate/base_estimator.hpp" -#include "ff_msgs/msg/free_flyer_state_stamped.hpp" -#include "ff_msgs/msg/pose2_d_stamped.hpp" - -namespace ff -{ - -/** - * @brief full-state estimator using only pose measurements - */ -class Pose2DEstimator : public BaseEstimator -{ -public: - explicit Pose2DEstimator(const std::string & node_name); - -protected: - /** - * @brief estimate state with Pose2D - * - * @param pose_stamped timestamped pose2d - */ - virtual void EstimateWithPose2D(const ff_msgs::msg::Pose2DStamped & pose_stamped); - -private: - ff_msgs::msg::FreeFlyerStateStamped prev_; - bool prev_state_ready_ = false; -}; - -} // namespace ff diff --git a/ff_estimate/src/base_estimator.cpp b/ff_estimate/src/base_mocap_estimator.cpp similarity index 61% rename from ff_estimate/src/base_estimator.cpp rename to ff_estimate/src/base_mocap_estimator.cpp index a53df2c..64c1585 100644 --- a/ff_estimate/src/base_estimator.cpp +++ b/ff_estimate/src/base_mocap_estimator.cpp @@ -21,21 +21,28 @@ // SOFTWARE. -#include "ff_estimate/base_estimator.hpp" +#include "ff_estimate/base_mocap_estimator.hpp" using ff_msgs::msg::FreeFlyerState; using ff_msgs::msg::FreeFlyerStateStamped; +using ff_msgs::msg::Pose2DStamped; +using geometry_msgs::msg::PoseStamped; namespace ff { -BaseEstimator::BaseEstimator(const std::string & node_name) +BaseMocapEstimator::BaseMocapEstimator(const std::string & node_name) : rclcpp::Node(node_name) { + const std::string pose_channel = this->declare_parameter("pose_channel", "mocap/sim/pose"); + pose_sub_ = this->create_subscription( + pose_channel, rclcpp::SensorDataQoS(), [this](const PoseStamped::SharedPtr msg) { + PoseCallback(msg); + }); state_pub_ = this->create_publisher("est/state", 10); } -void BaseEstimator::SendStateEstimate(const FreeFlyerState & state) +void BaseMocapEstimator::SendStateEstimate(const FreeFlyerState & state) { ff_msgs::msg::FreeFlyerStateStamped msg{}; msg.state = state; @@ -44,4 +51,19 @@ void BaseEstimator::SendStateEstimate(const FreeFlyerState & state) state_pub_->publish(msg); } +void BaseMocapEstimator::PoseCallback(const PoseStamped::SharedPtr pose) +{ + // convert to 3D pose to 2D pose + Pose2DStamped pose2d{}; + + pose2d.header = pose->header; + pose2d.pose.x = pose->pose.position.x; + pose2d.pose.y = pose->pose.position.y; + double w = pose->pose.orientation.w; + double z = pose->pose.orientation.z; + pose2d.pose.theta = std::atan2(2 * w * z, w * w - z * z); + + EstimateWithPose2D(pose2d); +} + } // namespace ff diff --git a/ff_estimate/src/mocap_estimator_node.cpp b/ff_estimate/src/mocap_estimator_node.cpp deleted file mode 100644 index cae5324..0000000 --- a/ff_estimate/src/mocap_estimator_node.cpp +++ /dev/null @@ -1,73 +0,0 @@ -// MIT License -// -// Copyright (c) 2023 Stanford Autonomous Systems Lab -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - - -#include -#include -#include - -#include "geometry_msgs/msg/pose_stamped.hpp" - -#include "ff_estimate/pose2d_estimator.hpp" -#include "ff_msgs/msg/pose2_d_stamped.hpp" - -using ff_msgs::msg::Pose2DStamped; -using geometry_msgs::msg::PoseStamped; - -class MocapEstimatorNode : public ff::Pose2DEstimator -{ -public: - MocapEstimatorNode() - : ff::Pose2DEstimator("mocap_estimator_node") - { - const std::string pose_channel = this->declare_parameter("pose_channel", "mocap/sim/pose"); - pose_sub_ = this->create_subscription( - pose_channel, rclcpp::SensorDataQoS(), [this](const PoseStamped::SharedPtr msg) { - PoseCallback(msg); - }); - } - -private: - rclcpp::Subscription::SharedPtr pose_sub_; - - void PoseCallback(const PoseStamped::SharedPtr pose) - { - // convert to pose2d - Pose2DStamped pose2d{}; - - pose2d.header = pose->header; - pose2d.pose.x = pose->pose.position.x; - pose2d.pose.y = pose->pose.position.y; - double w = pose->pose.orientation.w; - double z = pose->pose.orientation.z; - pose2d.pose.theta = std::atan2(2 * w * z, w * w - z * z); - - this->EstimateWithPose2D(pose2d); - } -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); -} diff --git a/ff_estimate/src/moving_avg_estimator_node.cpp b/ff_estimate/src/moving_avg_estimator_node.cpp new file mode 100644 index 0000000..1a4ed8d --- /dev/null +++ b/ff_estimate/src/moving_avg_estimator_node.cpp @@ -0,0 +1,91 @@ +// MIT License +// +// Copyright (c) 2023 Stanford Autonomous Systems Lab +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#include "ff_estimate/base_mocap_estimator.hpp" + +using ff_msgs::msg::FreeFlyerState; +using ff_msgs::msg::Pose2DStamped; + +class MovingAvgEstimatorNode : public ff::BaseMocapEstimator +{ +public: + MovingAvgEstimatorNode() + : ff::BaseMocapEstimator("moving_avg_estimator") + { + // perform simple 1st order IIR lowpass filter on derivate estimates + // coefficient in [0, 1) (higher coefficient filters better with larger delay) + this->declare_parameter("lowpass_coeff", .95); + this->declare_parameter("min_dt", 0.005); + } + + void EstimateWithPose2D(const Pose2DStamped & pose_stamped) override + { + FreeFlyerState state{}; + + state.pose = pose_stamped.pose; + if (prev_state_ready_) { + const rclcpp::Time now = pose_stamped.header.stamp; + const rclcpp::Time last = prev_.header.stamp; + double dt = (now - last).seconds(); + + // ignore this frame if it is too close to the last frame + if (dt < this->get_parameter("min_dt").as_double()) { + return; + } + + // finite difference + double vx = (pose_stamped.pose.x - prev_.state.pose.x) / dt; + double vy = (pose_stamped.pose.y - prev_.state.pose.y) / dt; + // wrap angle delta to [-pi, pi] + double dtheta = std::remainder(pose_stamped.pose.theta - prev_.state.pose.theta, 2 * M_PI); + double wz = dtheta / dt; + + double alpha = this->get_parameter("lowpass_coeff").as_double(); + if (alpha < 0 || alpha >= 1) { + RCLCPP_ERROR(this->get_logger(), "IIR filter disabled: invalid coefficient %f", alpha); + alpha = 0; + } + state.twist.vx = alpha * prev_.state.twist.vx + (1. - alpha) * vx; + state.twist.vy = alpha * prev_.state.twist.vy + (1. - alpha) * vy; + state.twist.wz = alpha * prev_.state.twist.wz + (1. - alpha) * wz; + } else { + prev_state_ready_ = true; + } + + prev_.state = state; + prev_.header = pose_stamped.header; + + SendStateEstimate(state); + } + +private: + ff_msgs::msg::FreeFlyerStateStamped prev_; + bool prev_state_ready_ = false; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} diff --git a/ff_estimate/src/pose2d_estimator.cpp b/ff_estimate/src/pose2d_estimator.cpp deleted file mode 100644 index c06abe9..0000000 --- a/ff_estimate/src/pose2d_estimator.cpp +++ /dev/null @@ -1,82 +0,0 @@ -// MIT License -// -// Copyright (c) 2023 Stanford Autonomous Systems Lab -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - - -#include "ff_estimate/pose2d_estimator.hpp" -#include "ff_msgs/msg/free_flyer_state.hpp" - -using ff_msgs::msg::FreeFlyerState; -using ff_msgs::msg::Pose2DStamped; - -namespace ff -{ - -Pose2DEstimator::Pose2DEstimator(const std::string & node_name) -: BaseEstimator(node_name) -{ - // perform simple 1st order IIR lowpass filter on derivate estimates - // coefficient in [0, 1) (higher coefficient filters better with larger delay) - this->declare_parameter("lowpass_coeff", .95); - this->declare_parameter("min_dt", 0.005); -} - -void Pose2DEstimator::EstimateWithPose2D(const Pose2DStamped & pose_stamped) -{ - FreeFlyerState state{}; - - state.pose = pose_stamped.pose; - if (prev_state_ready_) { - const rclcpp::Time now = pose_stamped.header.stamp; - const rclcpp::Time last = prev_.header.stamp; - double dt = (now - last).seconds(); - - // ignore this frame if it is too close to the last frame - if (dt < this->get_parameter("min_dt").as_double()) { - return; - } - - // finite difference - double vx = (pose_stamped.pose.x - prev_.state.pose.x) / dt; - double vy = (pose_stamped.pose.y - prev_.state.pose.y) / dt; - // wrap angle delta to [-pi, pi] - double dtheta = std::remainder(pose_stamped.pose.theta - prev_.state.pose.theta, 2 * M_PI); - double wz = dtheta / dt; - - double alpha = this->get_parameter("lowpass_coeff").as_double(); - if (alpha < 0 || alpha >= 1) { - RCLCPP_ERROR(this->get_logger(), "IIR filter disabled: invalid coefficient %f", alpha); - alpha = 0; - } - state.twist.vx = alpha * prev_.state.twist.vx + (1. - alpha) * vx; - state.twist.vy = alpha * prev_.state.twist.vy + (1. - alpha) * vy; - state.twist.wz = alpha * prev_.state.twist.wz + (1. - alpha) * wz; - } else { - prev_state_ready_ = true; - } - - prev_.state = state; - prev_.header = pose_stamped.header; - - SendStateEstimate(state); -} - -} // namespace ff diff --git a/freeflyer/launch/estimation_viz.launch.py b/freeflyer/launch/estimation_viz.launch.py index c70059d..e5326e1 100644 --- a/freeflyer/launch/estimation_viz.launch.py +++ b/freeflyer/launch/estimation_viz.launch.py @@ -46,8 +46,8 @@ def generate_launch_description(): ), Node( package="ff_estimate", - executable="mocap_estimator_node", - name="mocap_estimator_node", + executable="moving_avg_estimator_node", + name="moving_avg_estimator_node", namespace=robot_name, parameters=[ { diff --git a/freeflyer/launch/hardware_pd.launch.py b/freeflyer/launch/hardware_pd.launch.py index dcceee7..6e4f0a4 100644 --- a/freeflyer/launch/hardware_pd.launch.py +++ b/freeflyer/launch/hardware_pd.launch.py @@ -68,8 +68,8 @@ def generate_launch_description(): ), Node( package="ff_estimate", - executable="mocap_estimator_node", - name="mocap_estimator_node", + executable="moving_avg_estimator_node", + name="moving_avg_estimator_node", namespace=robot_name, parameters=[ { diff --git a/freeflyer/launch/sim_key_teleop.launch.py b/freeflyer/launch/sim_key_teleop.launch.py index d9ed798..eb9b103 100644 --- a/freeflyer/launch/sim_key_teleop.launch.py +++ b/freeflyer/launch/sim_key_teleop.launch.py @@ -63,8 +63,8 @@ def generate_launch_description(): ), Node( package="ff_estimate", - executable="mocap_estimator_node", - name="mocap_estimator_node", + executable="moving_avg_estimator_node", + name="moving_avg_estimator_node", namespace=robot_name, ), ] diff --git a/freeflyer/launch/sim_pd.launch.py b/freeflyer/launch/sim_pd.launch.py index 8f4c333..53fc139 100644 --- a/freeflyer/launch/sim_pd.launch.py +++ b/freeflyer/launch/sim_pd.launch.py @@ -69,8 +69,8 @@ def generate_launch_description(): ), Node( package="ff_estimate", - executable="mocap_estimator_node", - name="mocap_estimator_node", + executable="moving_avg_estimator_node", + name="moving_avg_estimator_node", namespace=robot_name, ), ] diff --git a/freeflyer/test/pd_ctrl_launch_test.py b/freeflyer/test/pd_ctrl_launch_test.py index a25dbc7..2949c13 100644 --- a/freeflyer/test/pd_ctrl_launch_test.py +++ b/freeflyer/test/pd_ctrl_launch_test.py @@ -58,10 +58,10 @@ def generate_test_description(): name="pd_ctrl_node", namespace=ROBOT_NAME, ) - mocap_estimator = Node( + estimator = Node( package="ff_estimate", - executable="mocap_estimator_node", - name="mocap_estimator_node", + executable="moving_avg_estimator_node", + name="moving_avg_estimator_node", namespace=ROBOT_NAME, ) @@ -70,7 +70,7 @@ def generate_test_description(): DeclareLaunchArgument("impl", default_value="cpp", choices=["cpp", "py"]), sim_launch, pd_ctrl_node, - mocap_estimator, + estimator, ReadyToTest(), ] )