From 476e75e3f3d17c35ac89b17f513d93078687d613 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 19 Jan 2024 13:53:57 +0000 Subject: [PATCH] make everything a hpp --- ...alibration.h => extrinsic_calibration.hpp} | 13 +- .../src/extrinsic_calibration.cpp | 2 +- .../{kick_engine.h => kick_engine.hpp} | 38 ++--- .../{kick_ik.h => kick_ik.hpp} | 13 +- .../{kick_node.h => kick_node.hpp} | 50 +++--- .../{kick_pywrapper.h => kick_pywrapper.hpp} | 17 +- .../{kick_utils.h => kick_utils.hpp} | 4 +- .../{stabilizer.h => stabilizer.hpp} | 20 ++- .../{visualizer.h => visualizer.hpp} | 32 ++-- .../bitbots_dynamic_kick/src/kick_engine.cpp | 2 +- .../bitbots_dynamic_kick/src/kick_ik.cpp | 2 +- .../bitbots_dynamic_kick/src/kick_node.cpp | 2 +- .../src/kick_pywrapper.cpp | 4 +- .../bitbots_dynamic_kick/src/stabilizer.cpp | 2 +- .../bitbots_dynamic_kick/src/visualizer.cpp | 2 +- .../{dynup_engine.h => dynup_engine.hpp} | 24 ++- .../{dynup_ik.h => dynup_ik.hpp} | 19 ++- .../include/bitbots_dynup/dynup_node.h | 156 ------------------ .../include/bitbots_dynup/dynup_node.hpp | 148 +++++++++++++++++ .../include/bitbots_dynup/dynup_pywrapper.h | 36 ---- .../include/bitbots_dynup/dynup_pywrapper.hpp | 35 ++++ ...ynup_stabilizer.h => dynup_stabilizer.hpp} | 18 +- .../{dynup_utils.h => dynup_utils.hpp} | 7 +- .../{visualizer.h => visualizer.hpp} | 17 +- .../bitbots_dynup/src/dynup_engine.cpp | 2 +- bitbots_motion/bitbots_dynup/src/dynup_ik.cpp | 5 +- .../bitbots_dynup/src/dynup_node.cpp | 3 +- .../bitbots_dynup/src/dynup_pywrapper.cpp | 2 +- .../bitbots_dynup/src/dynup_stabilizer.cpp | 2 +- .../bitbots_dynup/src/visualizer.cpp | 4 +- ...{motion_odometry.h => motion_odometry.hpp} | 18 +- .../{odometry_fuser.h => odometry_fuser.hpp} | 39 ++--- .../bitbots_odometry/src/motion_odometry.cpp | 2 +- .../bitbots_odometry/src/odometry_fuser.cpp | 2 +- .../{walk_engine.h => walk_engine.hpp} | 29 ++-- .../{walk_ik.h => walk_ik.hpp} | 13 +- .../{walk_node.h => walk_node.hpp} | 71 ++++---- .../{walk_pywrapper.h => walk_pywrapper.hpp} | 39 ++--- ...{walk_stabilizer.h => walk_stabilizer.hpp} | 19 ++- .../{walk_utils.h => walk_utils.hpp} | 23 +-- ...{walk_visualizer.h => walk_visualizer.hpp} | 40 ++--- .../bitbots_quintic_walk/src/walk_engine.cpp | 2 +- .../bitbots_quintic_walk/src/walk_ik.cpp | 2 +- .../bitbots_quintic_walk/src/walk_node.cpp | 2 +- .../src/walk_pywrapper.cpp | 2 +- .../src/walk_stabilizer.cpp | 2 +- .../src/walk_visualizer.cpp | 4 +- ...{abstract_engine.h => abstract_engine.hpp} | 6 +- .../include/bitbots_splines/abstract_ik.h | 92 ----------- .../include/bitbots_splines/abstract_ik.hpp | 89 ++++++++++ ...t_stabilizer.h => abstract_stabilizer.hpp} | 7 +- ...t_visualizer.h => abstract_visualizer.hpp} | 21 ++- .../{combination.h => combination.hpp} | 11 +- .../bitbots_splines/{euler.h => euler.hpp} | 77 ++++----- ...{newton_binomial.h => newton_binomial.hpp} | 7 +- .../{polynom.h => polynom.hpp} | 11 +- .../{pose_spline.h => pose_spline.hpp} | 11 +- ...{position_spline.h => position_spline.hpp} | 6 +- .../{smooth_spline.h => smooth_spline.hpp} | 14 +- ...pline_container.h => spline_container.hpp} | 71 +++----- .../bitbots_splines/src/Spline/polynom.cpp | 4 +- .../src/Spline/pose_spline.cpp | 4 +- .../src/Spline/position_spline.cpp | 2 +- .../src/Spline/smooth_spline.cpp | 2 +- .../bitbots_splines/src/Utils/combination.cpp | 2 +- .../src/Utils/newton_binomial.cpp | 2 +- 66 files changed, 669 insertions(+), 760 deletions(-) rename bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/{extrinsic_calibration.h => extrinsic_calibration.hpp} (99%) rename bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/{kick_engine.h => kick_engine.hpp} (88%) rename bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/{kick_ik.h => kick_ik.hpp} (81%) rename bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/{kick_node.h => kick_node.hpp} (87%) rename bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/{kick_pywrapper.h => kick_pywrapper.hpp} (81%) rename bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/{kick_utils.h => kick_utils.hpp} (91%) rename bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/{stabilizer.h => stabilizer.hpp} (81%) rename bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/{visualizer.h => visualizer.hpp} (82%) rename bitbots_motion/bitbots_dynup/include/bitbots_dynup/{dynup_engine.h => dynup_engine.hpp} (90%) rename bitbots_motion/bitbots_dynup/include/bitbots_dynup/{dynup_ik.h => dynup_ik.hpp} (82%) delete mode 100644 bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.h create mode 100644 bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp delete mode 100644 bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_pywrapper.h create mode 100644 bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_pywrapper.hpp rename bitbots_motion/bitbots_dynup/include/bitbots_dynup/{dynup_stabilizer.h => dynup_stabilizer.hpp} (87%) rename bitbots_motion/bitbots_dynup/include/bitbots_dynup/{dynup_utils.h => dynup_utils.hpp} (75%) rename bitbots_motion/bitbots_dynup/include/bitbots_dynup/{visualizer.h => visualizer.hpp} (91%) rename bitbots_motion/bitbots_odometry/include/bitbots_odometry/{motion_odometry.h => motion_odometry.hpp} (98%) rename bitbots_motion/bitbots_odometry/include/bitbots_odometry/{odometry_fuser.h => odometry_fuser.hpp} (96%) rename bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/{walk_engine.h => walk_engine.hpp} (90%) rename bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/{walk_ik.h => walk_ik.hpp} (87%) rename bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/{walk_node.h => walk_node.hpp} (94%) rename bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/{walk_pywrapper.h => walk_pywrapper.hpp} (62%) rename bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/{walk_stabilizer.h => walk_stabilizer.hpp} (90%) rename bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/{walk_utils.h => walk_utils.hpp} (88%) rename bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/{walk_visualizer.h => walk_visualizer.hpp} (75%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{abstract_engine.h => abstract_engine.hpp} (86%) delete mode 100644 bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_ik.h create mode 100644 bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_ik.hpp rename bitbots_motion/bitbots_splines/include/bitbots_splines/{abstract_stabilizer.h => abstract_stabilizer.hpp} (84%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{abstract_visualizer.h => abstract_visualizer.hpp} (91%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{combination.h => combination.hpp} (93%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{euler.h => euler.hpp} (78%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{newton_binomial.h => newton_binomial.hpp} (88%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{polynom.h => polynom.hpp} (94%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{pose_spline.h => pose_spline.hpp} (93%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{position_spline.h => position_spline.hpp} (86%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{smooth_spline.h => smooth_spline.hpp} (82%) rename bitbots_motion/bitbots_splines/include/bitbots_splines/{spline_container.h => spline_container.hpp} (79%) diff --git a/bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.h b/bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.hpp similarity index 99% rename from bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.h rename to bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.hpp index fc9a8bc70..610cfd063 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.h +++ b/bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.hpp @@ -1,12 +1,12 @@ -#include -#include +#include +#include +#include #include + #include -#include -#include -#include #include - +#include +#include #include using std::placeholders::_1; @@ -14,6 +14,7 @@ class ExtrinsicCalibrationBroadcaster : public rclcpp::Node { public: ExtrinsicCalibrationBroadcaster(); void step(); + private: OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; std::unique_ptr broadcaster_; diff --git a/bitbots_misc/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp b/bitbots_misc/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp index 116eec57f..82342cf27 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp +++ b/bitbots_misc/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp @@ -1,4 +1,4 @@ -#include +#include ExtrinsicCalibrationBroadcaster::ExtrinsicCalibrationBroadcaster() : Node("bitbots_extrinsic_calibration") { broadcaster_ = std::make_unique(this); diff --git a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.hpp similarity index 88% rename from bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h rename to bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.hpp index a6848b13c..1f84e4656 100644 --- a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.h +++ b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_engine.hpp @@ -1,19 +1,21 @@ #ifndef BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_ENGINE_H_ #define BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_ENGINE_H_ -#include -#include -#include #include -#include -#include -#include +#include +#include +#include + +#include #include +#include +#include +#include +#include +#include #include #include -#include -#include -#include + #include "stabilizer.h" #include "visualizer.h" @@ -69,7 +71,7 @@ class PhaseTimings { */ class KickEngine : public bitbots_splines::AbstractEngine { public: - KickEngine(rclcpp::Node::SharedPtr node); + explicit KickEngine(rclcpp::Node::SharedPtr node); /** * Set new goal which the engine tries to kick at. This will remove the old goal completely and plan new splines. @@ -167,8 +169,7 @@ class KickEngine : public bitbots_splines::AbstractEngine transformGoal( - const std::string &support_foot_frame, - const Eigen::Isometry3d &trunk_to_base_footprint, - const Eigen::Vector3d &ball_position, - const Eigen::Quaterniond &kick_direction); + std::pair transformGoal(const std::string &support_foot_frame, + const Eigen::Isometry3d &trunk_to_base_footprint, + const Eigen::Vector3d &ball_position, + const Eigen::Quaterniond &kick_direction); }; -} +} // namespace bitbots_dynamic_kick -#endif //BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_ENGINE_H_ +#endif // BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_ENGINE_H_ diff --git a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_ik.h b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_ik.hpp similarity index 81% rename from bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_ik.h rename to bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_ik.hpp index 2fc1bc2f2..894a05bd9 100644 --- a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_ik.h +++ b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_ik.hpp @@ -1,11 +1,12 @@ #ifndef BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_IK_H_ #define BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_IK_H_ -#include -#include #include -#include -#include +#include + +#include +#include +#include namespace bitbots_dynamic_kick { @@ -23,6 +24,6 @@ class KickIK : public bitbots_splines::AbstractIK { moveit::core::JointModelGroup *left_leg_joints_group_; moveit::core::JointModelGroup *right_leg_joints_group_; }; -} +} // namespace bitbots_dynamic_kick -#endif //BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_IK_H_ +#endif // BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_IK_H_ diff --git a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.hpp similarity index 87% rename from bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h rename to bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.hpp index 99243e596..926c23a90 100644 --- a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.h +++ b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_node.hpp @@ -1,26 +1,26 @@ #ifndef BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_NODE_H_ #define BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_NODE_H_ -#include -#include -#include -#include +#include +#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include +#include +#include +#include #include #include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include namespace bitbots_dynamic_kick { using KickGoal = bitbots_msgs::action::Kick; @@ -36,7 +36,7 @@ using namespace std::placeholders; * * Additionally it publishes the KickEngines motor-goals back into ROS */ -class KickNode : public rclcpp::Node{ +class KickNode : public rclcpp::Node { public: explicit KickNode(const std::string &ns = std::string(), std::vector parameters = {}); @@ -109,23 +109,23 @@ class KickNode : public rclcpp::Node{ KickParams normal_config_; /** - * Do main loop in which KickEngine::update() gets called repeatedly. - * The ActionServer's state is taken into account meaning that a cancelled goal no longer gets processed. - */ + * Do main loop in which KickEngine::update() gets called repeatedly. + * The ActionServer's state is taken into account meaning that a cancelled goal no longer gets processed. + */ void loopEngine(const std::shared_ptr> goal_handle); rclcpp_action::GoalResponse goalCb(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal); - rclcpp_action::CancelResponse - cancelCb(std::shared_ptr> goal); + rclcpp_action::CancelResponse cancelCb( + std::shared_ptr> goal); void acceptedCb(const std::shared_ptr goal); /** - * Execute one step of engine-stabilize-ik - * @return the motor goals - */ + * Execute one step of engine-stabilize-ik + * @return the motor goals + */ bitbots_splines::JointGoals kickStep(double dt); /** @@ -145,6 +145,6 @@ class KickNode : public rclcpp::Node{ rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector ¶meters); }; -} +} // namespace bitbots_dynamic_kick -#endif //BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_NODE_H_ +#endif // BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_NODE_H_ diff --git a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.h b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.hpp similarity index 81% rename from bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.h rename to bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.hpp index 8fdbeecff..5336cb7b5 100644 --- a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.h +++ b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_pywrapper.hpp @@ -1,15 +1,16 @@ #ifndef BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_PYWRAPPER_H_ #define BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_PYWRAPPER_H_ -#include -#include #include +#include + #include +#include +#include +#include +#include #include #include -#include -#include -#include class PyKickWrapper { public: @@ -17,8 +18,8 @@ class PyKickWrapper { moveit::py_bindings_tools::ByteString step(double dt, const std::string &joint_state_str); bool set_goal(const std::string &goal_str, const std::string &joint_state_str); double get_progress(); - //todo - // void set_params(boost::python::object params); + // todo + // void set_params(boost::python::object params); moveit::py_bindings_tools::ByteString get_trunk_pose(); bool is_left_kick(); @@ -26,4 +27,4 @@ class PyKickWrapper { std::shared_ptr kick_node_; }; -#endif //BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_PYWRAPPER_H_ +#endif // BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_PYWRAPPER_H_ diff --git a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.hpp similarity index 91% rename from bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h rename to bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.hpp index 494529142..7a6bc0069 100644 --- a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.h +++ b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/kick_utils.hpp @@ -33,6 +33,6 @@ enum KickPhase { DONE = bitbots_dynamic_kick::msg::KickDebug::DONE }; -} +} // namespace bitbots_dynamic_kick -#endif //BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_UTILS_H_ +#endif // BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_KICK_UTILS_H_ diff --git a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/stabilizer.h b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/stabilizer.hpp similarity index 81% rename from bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/stabilizer.h rename to bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/stabilizer.hpp index 258d1e759..422704c77 100644 --- a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/stabilizer.h +++ b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/stabilizer.hpp @@ -1,21 +1,22 @@ #ifndef BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_STABILIZER_H_ #define BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_STABILIZER_H_ -#include #include -#include #include -#include + +#include +#include +#include +#include + #include "kick_utils.h" #include "visualizer.h" -#include namespace bitbots_dynamic_kick { -class Stabilizer : - public bitbots_splines::AbstractStabilizer { +class Stabilizer : public bitbots_splines::AbstractStabilizer { public: - Stabilizer(std::string ns); + explicit Stabilizer(std::string ns); geometry_msgs::msg::Point cop_left; geometry_msgs::msg::Point cop_right; @@ -30,6 +31,7 @@ class Stabilizer : void reset() override; void useCop(bool use); void setRobotModel(moveit::core::RobotModelPtr model); + private: moveit::core::RobotModelPtr kinematic_model_; std::shared_ptr pid_trunk_fused_pitch_; @@ -39,6 +41,6 @@ class Stabilizer : bool use_cop_; }; -} +} // namespace bitbots_dynamic_kick -#endif //BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_STABILIZER_H_ +#endif // BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_STABILIZER_H_ diff --git a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.hpp similarity index 82% rename from bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h rename to bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.hpp index 8cbf9ac92..5471d9b91 100644 --- a/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.h +++ b/bitbots_motion/bitbots_dynamic_kick/include/bitbots_dynamic_kick/visualizer.hpp @@ -5,20 +5,21 @@ #ifndef BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_VISUALIZER_H_ #define BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_VISUALIZER_H_ -#include -#include -#include -#include -#include -#include -#include #include #include #include -#include -#include -#include + +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace bitbots_dynamic_kick { @@ -34,7 +35,6 @@ struct VisualizationParams { class Visualizer : bitbots_splines::AbstractVisualizer { public: - explicit Visualizer(const std::string &base_topic, rclcpp::Node::SharedPtr node); void setParams(VisualizationParams params); @@ -47,10 +47,8 @@ class Visualizer : bitbots_splines::AbstractVisualizer { void displayWindupPoint(const Eigen::Vector3d &kick_windup_point, const std::string &support_foot_frame); - void publishGoals(const KickPositions &positions, - const KickPositions &stabilized_positions, - const moveit::core::RobotStatePtr &robot_state, - KickPhase engine_phase); + void publishGoals(const KickPositions &positions, const KickPositions &stabilized_positions, + const moveit::core::RobotStatePtr &robot_state, KickPhase engine_phase); private: rclcpp::Node::SharedPtr node_; @@ -63,6 +61,6 @@ class Visualizer : bitbots_splines::AbstractVisualizer { const std::string marker_ns_ = "bitbots_dynamic_kick"; VisualizationParams params_; }; -} +} // namespace bitbots_dynamic_kick -#endif //BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_VISUALIZER_H_ +#endif // BITBOTS_DYNAMIC_KICK_INCLUDE_BITBOTS_DYNAMIC_KICK_VISUALIZER_H_ diff --git a/bitbots_motion/bitbots_dynamic_kick/src/kick_engine.cpp b/bitbots_motion/bitbots_dynamic_kick/src/kick_engine.cpp index c71aa0959..b09847137 100644 --- a/bitbots_motion/bitbots_dynamic_kick/src/kick_engine.cpp +++ b/bitbots_motion/bitbots_dynamic_kick/src/kick_engine.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynamic_kick/kick_engine.h" +#include "bitbots_dynamic_kick/kick_engine.hpp" #include diff --git a/bitbots_motion/bitbots_dynamic_kick/src/kick_ik.cpp b/bitbots_motion/bitbots_dynamic_kick/src/kick_ik.cpp index 710c4835c..a562c2da6 100644 --- a/bitbots_motion/bitbots_dynamic_kick/src/kick_ik.cpp +++ b/bitbots_motion/bitbots_dynamic_kick/src/kick_ik.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynamic_kick/kick_ik.h" +#include "bitbots_dynamic_kick/kick_ik.hpp" namespace bitbots_dynamic_kick { diff --git a/bitbots_motion/bitbots_dynamic_kick/src/kick_node.cpp b/bitbots_motion/bitbots_dynamic_kick/src/kick_node.cpp index 995126a11..89d85824f 100644 --- a/bitbots_motion/bitbots_dynamic_kick/src/kick_node.cpp +++ b/bitbots_motion/bitbots_dynamic_kick/src/kick_node.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynamic_kick/kick_node.h" +#include "bitbots_dynamic_kick/kick_node.hpp" namespace bitbots_dynamic_kick { using namespace std::chrono_literals; diff --git a/bitbots_motion/bitbots_dynamic_kick/src/kick_pywrapper.cpp b/bitbots_motion/bitbots_dynamic_kick/src/kick_pywrapper.cpp index ceffc03e2..7251f9e9f 100644 --- a/bitbots_motion/bitbots_dynamic_kick/src/kick_pywrapper.cpp +++ b/bitbots_motion/bitbots_dynamic_kick/src/kick_pywrapper.cpp @@ -1,4 +1,4 @@ -#include +#include /** * Read a ROS message from a serialized string. @@ -119,4 +119,4 @@ BOOST_PYTHON_MODULE (py_dynamic_kick) { .def("get_trunk_pose", &PyKickWrapper::get_trunk_pose) .def("is_left_kick", &PyKickWrapper::is_left_kick); } -*/ \ No newline at end of file +*/ diff --git a/bitbots_motion/bitbots_dynamic_kick/src/stabilizer.cpp b/bitbots_motion/bitbots_dynamic_kick/src/stabilizer.cpp index 214c69a7e..e78689de6 100644 --- a/bitbots_motion/bitbots_dynamic_kick/src/stabilizer.cpp +++ b/bitbots_motion/bitbots_dynamic_kick/src/stabilizer.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynamic_kick/stabilizer.h" +#include "bitbots_dynamic_kick/stabilizer.hpp" namespace bitbots_dynamic_kick { diff --git a/bitbots_motion/bitbots_dynamic_kick/src/visualizer.cpp b/bitbots_motion/bitbots_dynamic_kick/src/visualizer.cpp index 6a803aa9e..b65f01240 100644 --- a/bitbots_motion/bitbots_dynamic_kick/src/visualizer.cpp +++ b/bitbots_motion/bitbots_dynamic_kick/src/visualizer.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynamic_kick/visualizer.h" +#include "bitbots_dynamic_kick/visualizer.hpp" #include diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.h b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp similarity index 90% rename from bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.h rename to bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp index dd749733d..d1b9ce44a 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.h +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp @@ -1,25 +1,27 @@ #ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_ENGINE_H_ #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_ENGINE_H_ -#include "dynup_stabilizer.h" -#include #include #include #include #include +#include + +#include #include #include #include #include #include -#include #include +#include "dynup_stabilizer.h" + namespace bitbots_dynup { class DynupEngine : public bitbots_splines::AbstractEngine { public: - explicit DynupEngine(rclcpp::Node::SharedPtr node); + explicit DynupEngine(rclcpp::Node::SharedPtr node); void init(double arm_offset_y, double arm_offset_z); @@ -55,13 +57,8 @@ class DynupEngine : public bitbots_splines::AbstractEngine +#include +#include #include + #include + #include "dynup_utils.h" -#include -#include namespace bitbots_dynup { class DynupIK : public bitbots_splines::AbstractIK { public: + explicit DynupIK(rclcpp::Node::SharedPtr node); void init(moveit::core::RobotModelPtr kinematic_model) override; bitbots_splines::JointGoals calculate(const DynupResponse &ik_goals) override; void reset() override; @@ -19,8 +22,8 @@ class DynupIK : public bitbots_splines::AbstractIK { void setDirection(std::string direction); moveit::core::RobotStatePtr get_goal_state(); - -private: + private: + rclcpp::Node::SharedPtr node_; sensor_msgs::msg::JointState::SharedPtr current_joint_states_; moveit::core::JointModelGroup *all_joints_group_; moveit::core::JointModelGroup *l_arm_joints_group_; @@ -28,11 +31,11 @@ class DynupIK : public bitbots_splines::AbstractIK { moveit::core::JointModelGroup *r_arm_joints_group_; moveit::core::JointModelGroup *r_leg_joints_group_; moveit::core::RobotStatePtr goal_state_; - bool use_stabilizing_; - bool use_minimal_displacement_; + bool use_stabilizing_ = false; + bool use_minimal_displacement_ = false; std::string direction_; }; -} +} // namespace bitbots_dynup -#endif //BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_IK_H_ +#endif // BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_IK_H_ diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.h b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.h deleted file mode 100644 index 18be5e5df..000000000 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.h +++ /dev/null @@ -1,156 +0,0 @@ -#ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_NODE_H_ -#define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_NODE_H_ - -#include -#include -#include -#include - -#include - -#include "rclcpp_action/rclcpp_action.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include "bitbots_msgs/action/dynup.hpp" -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "bitbots_dynup/visualizer.h" -#include "bitbots_dynup/dynup_engine.h" -#include "bitbots_dynup/dynup_ik.h" -#include "bitbots_dynup/dynup_stabilizer.h" - -namespace bitbots_dynup { - using DynupGoal = bitbots_msgs::action::Dynup; - using DynupGoalHandle = rclcpp_action::ServerGoalHandle; - using namespace std::placeholders; - -/** - * DynupNode is that part of bitbots_dynamic_DynUp which takes care of interacting with ROS and utilizes a DynUpEngine - * to calculate actual DynUp behavior. - * - * It provides an ActionServer for the bitbots_msgs::DynUpAction. - * This actionServer accepts new goals in any tf frame, and sets up the DynUpEngines to work towards this new goal - * - * Additionally it publishes the DynUpEngines motor-goals back into ROS - */ - class DynupNode : public rclcpp::Node { - public: - explicit DynupNode(const std::string ns="", std::vector parameters = {}); - - rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector ¶meters); - - void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); - - void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr jointstates); - - DynupEngine *getEngine(); - - DynupIK *getIK(); - - /** - * Retrieve current positions of left foot and trunk relative to right foot - * - * @return The pair of (right foot, left foot) poses if transformation was successfull - */ - bitbots_dynup::msg::DynupPoses getCurrentPoses(); - - bitbots_msgs::msg::JointCommand step(double dt); - - bitbots_msgs::msg::JointCommand step(double dt, - const sensor_msgs::msg::Imu::SharedPtr imu_msg, - const sensor_msgs::msg::JointState::SharedPtr jointstate_msg); - - geometry_msgs::msg::PoseArray step_open_loop(double dt); - - void reset(int time = 0); - - private: - - /** - * Callback that gets executed whenever #m_server receives a new goal. - * @param goal New goal to process - */ - rclcpp_action::GoalResponse goalCb(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal); - - rclcpp_action::CancelResponse cancelCb(std::shared_ptr goal); - - void acceptedCb(const std::shared_ptr goal); - - rclcpp::Publisher::SharedPtr debug_publisher_; - rclcpp::Publisher::SharedPtr joint_goal_publisher_; - rclcpp::Subscription::SharedPtr cop_subscriber_; - rclcpp::Subscription::SharedPtr joint_state_subscriber_; - - std::vector param_names_; - rclcpp_action::Server::SharedPtr action_server_; - - OnSetParametersCallbackHandle::SharedPtr callback_handle_; - - DynupEngine engine_; - Stabilizer stabilizer_; - Visualizer visualizer_; - DynupIK ik_; - std::map params_; - int stable_duration_; - int engine_rate_; - int failed_tick_counter_; - double last_ros_update_time_; - double start_time_; - bool server_free_; - bool debug_; - std::unique_ptr tf_buffer_; - std::shared_ptr tf_listener_; - std::shared_ptr robot_model_loader_; - moveit::core::RobotModelPtr kinematic_model_; - - - std::string base_link_frame_, l_sole_frame_, r_sole_frame_, l_wrist_frame_, r_wrist_frame_; - - void execute(const std::shared_ptr goal); - - /** - * Do main loop in which DynUpEngine::tick() gets called repeatedly. - * The ActionServer's state is taken into account meaning that a cancelled goal no longer gets processed. - */ - void loopEngine(int, std::shared_ptr goal_handle); - - /** - * Publish the current support_foot so that a correct base_footprint can be calculated - * @param is_left_dyn_up Whether the left foot is the current DynUping foot, meaning it is in the air - */ - void publishSupportFoot(bool is_left_dyn_up); - - /** - * Creates the Goal Msg - */ - bitbots_msgs::msg::JointCommand createGoalMsg(const bitbots_splines::JointGoals &goals); - - /** - * Helper method to achieve correctly sampled rate - */ - double getTimeDelta(); - - - }; - -} - -#endif //BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_NODE_H_ diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp new file mode 100644 index 000000000..04fac6206 --- /dev/null +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp @@ -0,0 +1,148 @@ +#ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_NODE_H_ +#define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_NODE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bitbots_dynup/dynup_engine.h" +#include "bitbots_dynup/dynup_ik.h" +#include "bitbots_dynup/dynup_stabilizer.h" +#include "bitbots_dynup/visualizer.h" +#include "bitbots_msgs/action/dynup.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +namespace bitbots_dynup { +using DynupGoal = bitbots_msgs::action::Dynup; +using DynupGoalHandle = rclcpp_action::ServerGoalHandle; +using namespace std::placeholders; + +/** + * DynupNode is that part of bitbots_dynamic_DynUp which takes care of interacting with ROS and utilizes a DynUpEngine + * to calculate actual DynUp behavior. + * + * It provides an ActionServer for the bitbots_msgs::DynUpAction. + * This actionServer accepts new goals in any tf frame, and sets up the DynUpEngines to work towards this new goal + * + * Additionally it publishes the DynUpEngines motor-goals back into ROS + */ +class DynupNode : public rclcpp::Node { + public: + explicit DynupNode(const std::string &ns = "", std::vector parameters = {}); + + rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector ¶meters); + + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); + + void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr jointstates); + + DynupEngine *getEngine(); + + DynupIK *getIK(); + + /** + * Retrieve current positions of left foot and trunk relative to right foot + * + * @return The pair of (right foot, left foot) poses if transformation was successfull + */ + bitbots_dynup::msg::DynupPoses getCurrentPoses(); + + bitbots_msgs::msg::JointCommand step(double dt); + + bitbots_msgs::msg::JointCommand step(double dt, const sensor_msgs::msg::Imu::SharedPtr imu_msg, + const sensor_msgs::msg::JointState::SharedPtr jointstate_msg); + + geometry_msgs::msg::PoseArray step_open_loop(double dt); + + void reset(int time = 0); + + private: + /** + * Callback that gets executed whenever #m_server receives a new goal. + * @param goal New goal to process + */ + rclcpp_action::GoalResponse goalCb(const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse cancelCb(std::shared_ptr goal); + + void acceptedCb(const std::shared_ptr goal); + + rclcpp::Publisher::SharedPtr debug_publisher_; + rclcpp::Publisher::SharedPtr joint_goal_publisher_; + rclcpp::Subscription::SharedPtr cop_subscriber_; + rclcpp::Subscription::SharedPtr joint_state_subscriber_; + + std::vector param_names_; + rclcpp_action::Server::SharedPtr action_server_; + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + + DynupEngine engine_; + Stabilizer stabilizer_; + Visualizer visualizer_; + DynupIK ik_; + std::map params_; + int stable_duration_; + int engine_rate_; + int failed_tick_counter_; + double last_ros_update_time_; + double start_time_; + bool server_free_; + bool debug_; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr robot_model_loader_; + moveit::core::RobotModelPtr kinematic_model_; + + std::string base_link_frame_, l_sole_frame_, r_sole_frame_, l_wrist_frame_, r_wrist_frame_; + + void execute(const std::shared_ptr goal); + + /** + * Do main loop in which DynUpEngine::tick() gets called repeatedly. + * The ActionServer's state is taken into account meaning that a cancelled goal no longer gets processed. + */ + void loopEngine(int, std::shared_ptr goal_handle); + + /** + * Publish the current support_foot so that a correct base_footprint can be calculated + * @param is_left_dyn_up Whether the left foot is the current DynUping foot, meaning it is in the air + */ + void publishSupportFoot(bool is_left_dyn_up); + + /** + * Creates the Goal Msg + */ + bitbots_msgs::msg::JointCommand createGoalMsg(const bitbots_splines::JointGoals &goals); + + /** + * Helper method to achieve correctly sampled rate + */ + double getTimeDelta(); +}; + +} // namespace bitbots_dynup + +#endif // BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_NODE_H_ diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_pywrapper.h b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_pywrapper.h deleted file mode 100644 index fdf1a94df..000000000 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_pywrapper.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef BITBOTS_DYNUP_BITBOTS_DYNUP_SRC_DYNUP_PYWRAPPER_H -#define BITBOTS_DYNUP_BITBOTS_DYNUP_SRC_DYNUP_PYWRAPPER_H - -#include "bitbots_dynup/dynup_node.h" -#include -#include -#include -#include "bitbots_dynup/dynup_utils.h" -#include -#include -#include - -namespace py = pybind11; -using namespace ros2_python_extension; - -class PyDynupWrapper { -public: - PyDynupWrapper(std::string ns); - py::bytes step(double dt, - py::bytes &imu_msg, - py::bytes &jointstate_msg); - py::bytes step_open_loop(double dt); - py::bytes get_poses(); - void reset(); - void spin_some(); - void special_reset(double time); - void set_engine_goal(std::string direction); - int get_direction(); - void set_parameter(const py::bytes params); - -private: - std::shared_ptr dynup_node_; -}; - -#endif //BITBOTS_DYNUP_BITBOTS_DYNUP_SRC_DYNUP_PYWRAPPER_H - diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_pywrapper.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_pywrapper.hpp new file mode 100644 index 000000000..8f4a5e208 --- /dev/null +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_pywrapper.hpp @@ -0,0 +1,35 @@ +#ifndef BITBOTS_DYNUP_BITBOTS_DYNUP_SRC_DYNUP_PYWRAPPER_H +#define BITBOTS_DYNUP_BITBOTS_DYNUP_SRC_DYNUP_PYWRAPPER_H + +#include +#include + +#include +#include +#include +#include + +#include "bitbots_dynup/dynup_node.h" +#include "bitbots_dynup/dynup_utils.h" + +namespace py = pybind11; +using namespace ros2_python_extension; + +class PyDynupWrapper { + public: + explicit PyDynupWrapper(std::string ns); + py::bytes step(double dt, py::bytes &imu_msg, py::bytes &jointstate_msg); + py::bytes step_open_loop(double dt); + py::bytes get_poses(); + void reset(); + void spin_some(); + void special_reset(double time); + void set_engine_goal(std::string direction); + int get_direction(); + void set_parameter(const py::bytes params); + + private: + std::shared_ptr dynup_node_; +}; + +#endif // BITBOTS_DYNUP_BITBOTS_DYNUP_SRC_DYNUP_PYWRAPPER_H diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.h b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.hpp similarity index 87% rename from bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.h rename to bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.hpp index 524c34f31..2fec981d3 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.h +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_stabilizer.hpp @@ -1,15 +1,17 @@ #ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_STABILIZER_H_ #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_STABILIZER_H_ -#include -#include #include -#include "dynup_utils.h" -#include #include + #include -#include +#include +#include +#include #include +#include + +#include "dynup_utils.h" namespace bitbots_dynup { @@ -29,7 +31,7 @@ class Stabilizer : public bitbots_splines::AbstractStabilizer { std::shared_ptr pid_trunk_roll_; std::shared_ptr pitch_node_; std::shared_ptr roll_node_; - //Transform from r_sole frame to base_link frame, as we want to stabilize the base link. + // Transform from r_sole frame to base_link frame, as we want to stabilize the base link. geometry_msgs::msg::TransformStamped r_sole_to_trunk_; bool stabilize_now_; @@ -40,6 +42,6 @@ class Stabilizer : public bitbots_splines::AbstractStabilizer { bool use_stabilizing_; }; -} +} // namespace bitbots_dynup -#endif //BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_STABILIZER_H_ +#endif // BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_STABILIZER_H_ diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.h b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.hpp similarity index 75% rename from bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.h rename to bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.hpp index 0ddccafba..059cf8ff8 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.h +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_utils.hpp @@ -2,10 +2,11 @@ #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_UTILS_H_ #include + #include struct DynupResponse { - tf2::Transform l_foot_goal_pose; //relative to r_foot_goal_pose + tf2::Transform l_foot_goal_pose; // relative to r_foot_goal_pose tf2::Transform r_foot_goal_pose; tf2::Transform l_hand_goal_pose; tf2::Transform r_hand_goal_pose; @@ -16,10 +17,10 @@ struct DynupResponse { struct DynupRequest { /* Whether the robot should stand up from the front, back or from squad */ std::string direction; - geometry_msgs::msg::Pose l_foot_pose; //relative to r_foot_pose + geometry_msgs::msg::Pose l_foot_pose; // relative to r_foot_pose geometry_msgs::msg::Pose r_foot_pose; geometry_msgs::msg::Pose l_hand_pose; geometry_msgs::msg::Pose r_hand_pose; }; -#endif //BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_UTILS_H_ +#endif // BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_UTILS_H_ diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.h b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.hpp similarity index 91% rename from bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.h rename to bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.hpp index d77feed04..bb2bac1e3 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.h +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.hpp @@ -1,14 +1,15 @@ #ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_ #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_ -#include -#include -#include +#include #include #include -#include #include -#include + +#include +#include +#include +#include namespace bitbots_dynup { @@ -18,7 +19,6 @@ struct VisualizationParams { class Visualizer : bitbots_splines::AbstractVisualizer { public: - Visualizer(const std::string &base_topic, rclcpp::Node::SharedPtr node); void setParams(VisualizationParams params); @@ -26,13 +26,12 @@ class Visualizer : bitbots_splines::AbstractVisualizer { void displaySplines(bitbots_splines::PoseSpline splines, const std::string &frame); private: - rclcpp::Publisher::SharedPtr spline_publisher_; std::string base_topic_; rclcpp::Node::SharedPtr node_; const std::string marker_ns_ = "bitbots_dynup"; VisualizationParams params_; }; -} +} // namespace bitbots_dynup -#endif //BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_ +#endif // BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_ diff --git a/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp b/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp index bfa9c2827..38745681b 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynup/dynup_engine.h" +#include "bitbots_dynup/dynup_engine.hpp" #include diff --git a/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp b/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp index 7ea0c7b1f..425cb33e3 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp @@ -1,5 +1,8 @@ -#include +#include namespace bitbots_dynup { + +DynupIK::DynupIK(rclcpp::Node::SharedPtr node) : node_(node) {} + void DynupIK::init(moveit::core::RobotModelPtr kinematic_model) { current_joint_states_ = std::make_shared(); /* Extract joint groups from kinematics model */ diff --git a/bitbots_motion/bitbots_dynup/src/dynup_node.cpp b/bitbots_motion/bitbots_dynup/src/dynup_node.cpp index c6d4fc0da..e46c20bdb 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_node.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_node.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynup/dynup_node.h" +#include "bitbots_dynup/dynup_node.hpp" namespace bitbots_dynup { using namespace std::chrono_literals; @@ -11,6 +11,7 @@ DynupNode::DynupNode(const std::string ns, std::vector parame engine_(SharedPtr(this)), stabilizer_(ns), visualizer_("debug/dynup", SharedPtr(this)), + ik_(SharedPtr(this)), tf_buffer_(std::make_unique(this->get_clock())) { // get all kinematics parameters from the move_group node if they are not set manually via constructor std::string check_kinematic_parameters; diff --git a/bitbots_motion/bitbots_dynup/src/dynup_pywrapper.cpp b/bitbots_motion/bitbots_dynup/src/dynup_pywrapper.cpp index 7ba8ee67c..bc971f2a8 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_pywrapper.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_pywrapper.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynup/dynup_pywrapper.h" +#include "bitbots_dynup/dynup_pywrapper.hpp" PyDynupWrapper::PyDynupWrapper(const std::string ns) { // initialize rclcpp if not already done diff --git a/bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp b/bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp index 921fe1144..7b4effe89 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynup/dynup_stabilizer.h" +#include "bitbots_dynup/dynup_stabilizer.hpp" namespace bitbots_dynup { diff --git a/bitbots_motion/bitbots_dynup/src/visualizer.cpp b/bitbots_motion/bitbots_dynup/src/visualizer.cpp index a644c5835..8c2b48f18 100644 --- a/bitbots_motion/bitbots_dynup/src/visualizer.cpp +++ b/bitbots_motion/bitbots_dynup/src/visualizer.cpp @@ -1,4 +1,4 @@ -#include "bitbots_dynup/visualizer.h" +#include "bitbots_dynup/visualizer.hpp" #include @@ -24,4 +24,4 @@ void Visualizer::displaySplines(bitbots_splines::PoseSpline splines, const std:: spline_publisher_->publish(path); } -} // namespace bitbots_dynup \ No newline at end of file +} // namespace bitbots_dynup diff --git a/bitbots_motion/bitbots_odometry/include/bitbots_odometry/motion_odometry.h b/bitbots_motion/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp similarity index 98% rename from bitbots_motion/bitbots_odometry/include/bitbots_odometry/motion_odometry.h rename to bitbots_motion/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp index 9de619119..207cac4b6 100644 --- a/bitbots_motion/bitbots_odometry/include/bitbots_odometry/motion_odometry.h +++ b/bitbots_motion/bitbots_odometry/include/bitbots_odometry/motion_odometry.hpp @@ -1,18 +1,19 @@ -#include -#include "odometry_parameters.hpp" +#include +#include +#include +#include +#include #include #include #include #include +#include #include #include #include -#include -#include -#include -#include -#include + +#include "odometry_parameters.hpp" using std::placeholders::_1; @@ -22,6 +23,7 @@ class MotionOdometry : public rclcpp::Node { public: MotionOdometry(); void loop(); + private: rclcpp::Time joint_update_time_{rclcpp::Time(0, 0, RCL_ROS_TIME)}; char current_support_state_; @@ -52,4 +54,4 @@ class MotionOdometry : public rclcpp::Node { rclcpp::Time start_time_; }; -} +} // namespace bitbots_odometry diff --git a/bitbots_motion/bitbots_odometry/include/bitbots_odometry/odometry_fuser.h b/bitbots_motion/bitbots_odometry/include/bitbots_odometry/odometry_fuser.hpp similarity index 96% rename from bitbots_motion/bitbots_odometry/include/bitbots_odometry/odometry_fuser.h rename to bitbots_motion/bitbots_odometry/include/bitbots_odometry/odometry_fuser.hpp index 1c9a7a9b4..c9653e012 100644 --- a/bitbots_motion/bitbots_odometry/include/bitbots_odometry/odometry_fuser.h +++ b/bitbots_motion/bitbots_odometry/include/bitbots_odometry/odometry_fuser.hpp @@ -1,33 +1,33 @@ -#include - -#include -#include -#include -#include -#include #include #include #include #include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include #include #include #include #include #include +#include +#include +#include #include -using std::placeholders::_1; +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + using bitbots_utils::wait_for_tf; +using std::placeholders::_1; typedef message_filters::sync_policies::ApproximateTime SyncPolicy; @@ -35,6 +35,7 @@ class OdometryFuser : public rclcpp::Node { public: OdometryFuser(); void loop(); + private: sensor_msgs::msg::Imu imu_data_; nav_msgs::msg::Odometry odom_data_; @@ -50,7 +51,8 @@ class OdometryFuser : public rclcpp::Node { message_filters::Cache support_state_cache_; void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg); - void imuCallback(const sensor_msgs::msg::Imu::SharedPtr &img_msg, const nav_msgs::msg::Odometry::SharedPtr &motion_odom_msg); + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr &img_msg, + const nav_msgs::msg::Odometry::SharedPtr &motion_odom_msg); tf2::Quaternion getCurrentMotionOdomYaw(tf2::Quaternion motion_odom_rotation); tf2::Quaternion getCurrentImuRotationWithoutYaw(tf2::Quaternion imu_rotation); tf2::Transform getCurrentRotationPoint(); @@ -62,4 +64,3 @@ class OdometryFuser : public rclcpp::Node { rclcpp::Time start_time_; message_filters::Synchronizer sync_; }; - diff --git a/bitbots_motion/bitbots_odometry/src/motion_odometry.cpp b/bitbots_motion/bitbots_odometry/src/motion_odometry.cpp index 9cfa25ee5..f6445168d 100644 --- a/bitbots_motion/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_motion/bitbots_odometry/src/motion_odometry.cpp @@ -1,4 +1,4 @@ -#include +#include namespace bitbots_odometry { diff --git a/bitbots_motion/bitbots_odometry/src/odometry_fuser.cpp b/bitbots_motion/bitbots_odometry/src/odometry_fuser.cpp index 70e8f2ce7..cbce18cf9 100644 --- a/bitbots_motion/bitbots_odometry/src/odometry_fuser.cpp +++ b/bitbots_motion/bitbots_odometry/src/odometry_fuser.cpp @@ -1,4 +1,4 @@ -#include +#include /* odom -> baselink diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.h b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp similarity index 90% rename from bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.h rename to bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp index 9bd4aa290..6e00b91c3 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.h +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp @@ -6,17 +6,16 @@ The original files can be found at: #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_ENGINE_H_ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_ENGINE_H_ -#include #include #include -#include "bitbots_splines/abstract_engine.h" -#include "bitbots_splines/smooth_spline.h" -#include "bitbots_splines/pose_spline.h" +#include -#include "bitbots_quintic_walk/walk_utils.h" +#include "bitbots_quintic_walk/walk_utils.hpp" #include "bitbots_quintic_walk_parameters.hpp" - +#include "bitbots_splines/abstract_engine.hpp" +#include "bitbots_splines/pose_spline.hpp" +#include "bitbots_splines/smooth_spline.hpp" namespace bitbots_quintic_walk { @@ -39,11 +38,7 @@ class WalkEngine : public bitbots_splines::AbstractEngine step, - bool stop_walk, - bool walkable_state, + void reset(WalkState state, double phase, std::vector step, bool stop_walk, bool walkable_state, bool reset_odometry); /** @@ -99,7 +94,6 @@ class WalkEngine : public bitbots_splines::AbstractEngine #include + +#include + +#include "bitbots_quintic_walk/walk_utils.h" #include "bitbots_splines/abstract_ik.h" namespace bitbots_quintic_walk { class WalkIK : public bitbots_splines::AbstractIK { public: - WalkIK(rclcpp::Node::SharedPtr node); + explicit WalkIK(rclcpp::Node::SharedPtr node); bitbots_splines::JointGoals calculate(const WalkResponse &ik_goals); void init(moveit::core::RobotModelPtr kinematic_model) override; @@ -28,7 +30,6 @@ class WalkIK : public bitbots_splines::AbstractIK { const moveit::core::JointModelGroup *right_leg_joints_group_; double ik_timeout_; - }; -} // namespace bitbots_quintic_walk -#endif //BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ +} // namespace bitbots_quintic_walk +#endif // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp similarity index 94% rename from bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h rename to bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp index 02d6a28ae..410c9eca8 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp @@ -6,50 +6,48 @@ The original files can be found at: #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_NODE_H_ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_NODE_H_ -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include - -#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include "biped_interfaces/msg/phase.hpp" #include "bitbots_msgs/msg/foot_pressure.hpp" #include "bitbots_msgs/msg/joint_command.hpp" #include "bitbots_msgs/msg/robot_control_state.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include "bitbots_quintic_walk/walk_engine.h" -#include "bitbots_quintic_walk/walk_stabilizer.h" #include "bitbots_quintic_walk/walk_ik.h" -#include "bitbots_splines/abstract_ik.h" +#include "bitbots_quintic_walk/walk_stabilizer.h" #include "bitbots_quintic_walk/walk_visualizer.h" -#include #include "bitbots_quintic_walk_parameters.hpp" +#include "bitbots_splines/abstract_ik.h" namespace bitbots_quintic_walk { @@ -57,14 +55,12 @@ class WalkNode : public rclcpp::Node { public: explicit WalkNode(std::string ns = "", std::vector parameters = {}); bitbots_msgs::msg::JointCommand step(double dt); - bitbots_msgs::msg::JointCommand step(double dt, - geometry_msgs::msg::Twist::SharedPtr cmdvel_msg, + bitbots_msgs::msg::JointCommand step(double dt, geometry_msgs::msg::Twist::SharedPtr cmdvel_msg, sensor_msgs::msg::Imu::SharedPtr imu_msg, sensor_msgs::msg::JointState::SharedPtr jointstate_msg, bitbots_msgs::msg::FootPressure::SharedPtr pressure_left, bitbots_msgs::msg::FootPressure::SharedPtr pressure_right); - bitbots_msgs::msg::JointCommand step_relative(double dt, - geometry_msgs::msg::Twist::SharedPtr step_msg, + bitbots_msgs::msg::JointCommand step_relative(double dt, geometry_msgs::msg::Twist::SharedPtr step_msg, sensor_msgs::msg::Imu::SharedPtr imu_msg, sensor_msgs::msg::JointState::SharedPtr jointstate_msg, bitbots_msgs::msg::FootPressure::SharedPtr pressure_left, @@ -201,9 +197,8 @@ class WalkNode : public rclcpp::Node { double pitch_vel_; bool got_new_goals_; - }; -} // namespace bitbots_quintic_walk +} // namespace bitbots_quintic_walk -#endif // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_NODE_H_ +#endif // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_NODE_H_ diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.h b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.hpp similarity index 62% rename from bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.h rename to bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.hpp index 1eece0a40..3f0379a21 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.h +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.hpp @@ -1,39 +1,34 @@ #ifndef BITBOTS_QUINTIC_WALK_BITBOTS_QUINTIC_WALK_SRC_WALK_PYWRAPPER_H_ #define BITBOTS_QUINTIC_WALK_BITBOTS_QUINTIC_WALK_SRC_WALK_PYWRAPPER_H_ -#include "bitbots_quintic_walk/walk_node.h" -#include -#include -#include -#include "bitbots_quintic_walk/walk_utils.h" +#include +#include +#include + #include #include #include #include #include #include -#include -#include -#include +#include +#include +#include #include +#include "bitbots_quintic_walk/walk_node.hpp" +#include "bitbots_quintic_walk/walk_utils.hpp" + namespace py = pybind11; using namespace ros2_python_extension; class PyWalkWrapper { public: - PyWalkWrapper(std::string ns, std::vector parameter_msgs = {}, bool force_smooth_step_transition=false); - py::bytes step(double dt, - py::bytes &cmdvel_msg, - py::bytes &imu_msg, - py::bytes &jointstate_msg, - py::bytes &pressure_left, - py::bytes &pressure_right); - py::bytes step_relative(double dt, - py::bytes &step_msg, - py::bytes &imu_msg, - py::bytes &jointstate_msg, - py::bytes &pressure_left, - py::bytes &pressure_right); + explicit PyWalkWrapper(std::string ns, std::vector parameter_msgs = {}, + bool force_smooth_step_transition = false); + py::bytes step(double dt, py::bytes &cmdvel_msg, py::bytes &imu_msg, py::bytes &jointstate_msg, + py::bytes &pressure_left, py::bytes &pressure_right); + py::bytes step_relative(double dt, py::bytes &step_msg, py::bytes &imu_msg, py::bytes &jointstate_msg, + py::bytes &pressure_left, py::bytes &pressure_right); py::bytes step_open_loop(double dt, py::bytes &cmdvel_msg); py::bytes get_left_foot_pose(); py::bytes get_right_foot_pose(); @@ -54,4 +49,4 @@ class PyWalkWrapper { std::shared_ptr walk_node_; }; -#endif //BITBOTS_QUINTIC_WALK_BITBOTS_QUINTIC_WALK_SRC_WALK_PYWRAPPER_H_ +#endif // BITBOTS_QUINTIC_WALK_BITBOTS_QUINTIC_WALK_SRC_WALK_PYWRAPPER_H_ diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_stabilizer.h b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_stabilizer.hpp similarity index 90% rename from bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_stabilizer.h rename to bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_stabilizer.hpp index 31d939235..6831cf7e4 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_stabilizer.h +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_stabilizer.hpp @@ -1,17 +1,18 @@ #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_STABILIZER_H_ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_STABILIZER_H_ -#include "bitbots_splines/abstract_stabilizer.h" - -#include -#include #include -#include "bitbots_quintic_walk/walk_utils.h" #include + #include -#include -#include +#include +#include #include +#include +#include + +#include "bitbots_quintic_walk/walk_utils.h" +#include "bitbots_splines/abstract_stabilizer.h" namespace bitbots_quintic_walk { @@ -27,6 +28,6 @@ class WalkStabilizer : public bitbots_splines::AbstractStabilizer std::shared_ptr pitch_node_; std::shared_ptr roll_node_; }; -} // namespace bitbots_quintic_walk +} // namespace bitbots_quintic_walk -#endif //BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_STABILIZER_H_ \ No newline at end of file +#endif // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_STABILIZER_H_ \ No newline at end of file diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.h b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.hpp similarity index 88% rename from bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.h rename to bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.hpp index fe6031db4..96ffe4161 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.h +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_utils.hpp @@ -13,16 +13,7 @@ namespace bitbots_quintic_walk { -enum WalkState { - PAUSED, - WALKING, - IDLE, - START_MOVEMENT, - STOP_MOVEMENT, - START_STEP, - STOP_STEP, - KICK -}; +enum WalkState { PAUSED, WALKING, IDLE, START_MOVEMENT, STOP_MOVEMENT, START_STEP, STOP_STEP, KICK }; struct WalkRequest { std::vector linear_orders = {0, 0, 0}; @@ -65,11 +56,7 @@ struct WalkResponse { * Return the given angle in radian * bounded between -TAU/2 and TAU/2 */ -inline double angleBound(double angle) { - return - angle - - M_TAU * std::floor((angle + M_TAU / 2) / M_TAU); -} +inline double angleBound(double angle) { return angle - M_TAU * std::floor((angle + M_TAU / 2) / M_TAU); } /** * Compute the oriented distance between the two given angle @@ -107,7 +94,7 @@ inline double angleDistance(double angle_src, double angle_dst) { } } -inline void tf_pose_to_msg(tf2::Transform &tf_pose, geometry_msgs::msg::Pose &msg_pose){ +inline void tf_pose_to_msg(tf2::Transform &tf_pose, geometry_msgs::msg::Pose &msg_pose) { msg_pose.position.x = tf_pose.getOrigin().getX(); msg_pose.position.y = tf_pose.getOrigin().getY(); msg_pose.position.z = tf_pose.getOrigin().getZ(); @@ -117,6 +104,6 @@ inline void tf_pose_to_msg(tf2::Transform &tf_pose, geometry_msgs::msg::Pose &ms msg_pose.orientation.w = tf_pose.getRotation().getW(); } -} // namespace bitbots_quintic_walk +} // namespace bitbots_quintic_walk -#endif //BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_UTILS_H_ \ No newline at end of file +#endif // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_UTILS_H_ \ No newline at end of file diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.h b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.hpp similarity index 75% rename from bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.h rename to bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.hpp index 0e788956e..efe7b97af 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.h +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_visualizer.hpp @@ -1,48 +1,40 @@ #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_VISUALIZER_H_ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_VISUALIZER_H_ -#include - -#include "bitbots_quintic_walk/msg/walk_debug.hpp" -#include "bitbots_quintic_walk/msg/walk_engine_debug.hpp" -#include #include - -#include -#include +#include +#include +#include #include #include -#include -#include -#include +#include + #include +#include +#include +#include -#include "bitbots_splines/abstract_visualizer.h" +#include "bitbots_quintic_walk/msg/walk_debug.hpp" +#include "bitbots_quintic_walk/msg/walk_engine_debug.hpp" #include "bitbots_splines/abstract_ik.h" +#include "bitbots_splines/abstract_visualizer.h" namespace bitbots_quintic_walk { class WalkVisualizer : public bitbots_splines::AbstractVisualizer { public: explicit WalkVisualizer(rclcpp::Node::SharedPtr node); - void publishArrowMarker(std::string name_space, - std::string frame, - geometry_msgs::msg::Pose pose, - float r, - float g, - float b, - float a); + void publishArrowMarker(std::string name_space, std::string frame, geometry_msgs::msg::Pose pose, float r, float g, + float b, float a); void publishEngineDebug(WalkResponse response); - void publishIKDebug(WalkResponse response, - moveit::core::RobotStatePtr current_state, + void publishIKDebug(WalkResponse response, moveit::core::RobotStatePtr current_state, bitbots_splines::JointGoals joint_goals); void publishWalkMarkers(WalkResponse response); void init(moveit::core::RobotModelPtr kinematic_model); private: - rclcpp::Node::SharedPtr node_; int marker_id_; @@ -54,6 +46,6 @@ class WalkVisualizer : public bitbots_splines::AbstractVisualizer { std::string base_link_frame_, l_sole_frame_, r_sole_frame_; }; -} // namespace bitbots_quintic_walk +} // namespace bitbots_quintic_walk -#endif //BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_VISUALIZER_H_ \ No newline at end of file +#endif // BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_VISUALIZER_H_ \ No newline at end of file diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp index 99a4bcf9e..005d0c0f5 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp @@ -3,7 +3,7 @@ This code is partly based on the original code by Quentin "Leph" Rouxel and Team The original files can be found at: https://github.com/Rhoban/model/ */ -#include "bitbots_quintic_walk/walk_engine.h" +#include "bitbots_quintic_walk/walk_engine.hpp" namespace bitbots_quintic_walk { diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp index c46bbc6ec..86ced123d 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp @@ -1,4 +1,4 @@ -#include "bitbots_quintic_walk/walk_ik.h" +#include "bitbots_quintic_walk/walk_ik.hpp" namespace bitbots_quintic_walk { diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp index 7c29c4838..de60bbc5d 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp @@ -1,6 +1,6 @@ #define M_TAU M_PI * 2 -#include "bitbots_quintic_walk/walk_node.h" +#include "bitbots_quintic_walk/walk_node.hpp" #include #include diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_pywrapper.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_pywrapper.cpp index d85302364..22826b232 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_pywrapper.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_pywrapper.cpp @@ -1,4 +1,4 @@ -#include "bitbots_quintic_walk/walk_pywrapper.h" +#include "bitbots_quintic_walk/walk_pywrapper.hpp" void PyWalkWrapper::spin_some() { rclcpp::spin_some(walk_node_); } diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp index 6e1fda8d2..44957dc11 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp @@ -1,4 +1,4 @@ -#include "bitbots_quintic_walk/walk_stabilizer.h" +#include "bitbots_quintic_walk/walk_stabilizer.hpp" namespace bitbots_quintic_walk { diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp index 4dba91e14..454e545ec 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_visualizer.cpp @@ -1,4 +1,4 @@ -#include "bitbots_quintic_walk/walk_visualizer.h" +#include "bitbots_quintic_walk/walk_visualizer.hpp" namespace bitbots_quintic_walk { WalkVisualizer::WalkVisualizer(rclcpp::Node::SharedPtr node) : node_(node) { @@ -400,4 +400,4 @@ void WalkVisualizer::publishWalkMarkers(WalkResponse response) { marker_id_++; } -} // namespace bitbots_quintic_walk \ No newline at end of file +} // namespace bitbots_quintic_walk diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_engine.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_engine.hpp similarity index 86% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_engine.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_engine.hpp index 1c610becb..f1f15e06d 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_engine.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_engine.hpp @@ -5,7 +5,7 @@ namespace bitbots_splines { -template +template class AbstractEngine { public: /** @@ -29,6 +29,6 @@ class AbstractEngine { */ virtual int getPercentDone() const = 0; }; -} +} // namespace bitbots_splines -#endif //BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_ENGINE_H_ +#endif // BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_ENGINE_H_ diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_ik.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_ik.h deleted file mode 100644 index 72955c683..000000000 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_ik.h +++ /dev/null @@ -1,92 +0,0 @@ - -#ifndef BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_IK_H_ -#define BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_IK_H_ - -#include -#include -#include -#include - -namespace bitbots_splines { - - typedef std::pair, std::vector> JointGoals; - -/** - * Update joint goal values in bulk - * - * @param goals Initial goal values - * @param names List of names which include the joints that should get updated - * @param values New values for the previously defined joint names - * @return new joint goals with updated values - */ - inline JointGoals joint_goals_update(const JointGoals &goals, - const std::vector &names, - const std::vector &values, - rclcpp::Logger logger) { - JointGoals result = goals; - - if (names.size() != values.size()) { - RCLCPP_ERROR(logger, "joint_goals_update() called with unequal argument sizes"); - } - - for (size_t i = 0; i < goals.first.size(); ++i) { - for (size_t j = 0; j < names.size() && j < values.size(); ++j) { - if (result.first.at(i) == names.at(j)) { - result.second.at(i) = values.at(j); - } - } - } - - return result; - } - -/** - * Update joint goal values in bulk by adding the corresponding difference to the current value - * - * @param goals Initial goal values - * @param names List of names which include the joints that should get updated - * @param diffs Differences to be applied to the previously defined joint names - * @return new joint goals with updated values - */ - inline JointGoals joint_goals_update_diff(const JointGoals &goals, - const std::vector &names, - const std::vector &diffs, - rclcpp::Logger logger) { - JointGoals result = goals; - - if (names.size() != diffs.size()) { - RCLCPP_ERROR(logger, "joint_goals_update() called with unequal argument sizes"); - } - - for (size_t i = 0; i < goals.first.size(); ++i) { - for (size_t j = 0; j < names.size() && j < diffs.size(); ++j) { - if (result.first.at(i) == names.at(j)) { - result.second.at(i) += diffs.at(j); - } - } - } - - return result; - } - - template - class AbstractIK { - /** - * Initializes the class. This must be called before calculate() is called. - * @param kinematic_model The MoveIt! kinematic model of the robot - */ - virtual void init(moveit::core::RobotModelPtr kinematic_model) = 0; - /** - * Calculate motor joint goals from cartesian positions, i.e. solve the presented inverse kinematics problem - * @param positions the cartesian positions of end points and custom additional information (e.g. support foot) - * @return motor positions - */ - virtual JointGoals calculate(const Positions &positions) = 0; - /** - * Reset the IK to its initial state. - */ - virtual void reset() = 0; - }; -} - -#endif //BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_IK_H_ diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_ik.hpp b/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_ik.hpp new file mode 100644 index 000000000..3be7d2bed --- /dev/null +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_ik.hpp @@ -0,0 +1,89 @@ + +#ifndef BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_IK_H_ +#define BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_IK_H_ + +#include + +#include +#include +#include + +namespace bitbots_splines { + +typedef std::pair, std::vector> JointGoals; + +/** + * Update joint goal values in bulk + * + * @param goals Initial goal values + * @param names List of names which include the joints that should get updated + * @param values New values for the previously defined joint names + * @return new joint goals with updated values + */ +inline JointGoals joint_goals_update(const JointGoals &goals, const std::vector &names, + const std::vector &values, rclcpp::Logger logger) { + JointGoals result = goals; + + if (names.size() != values.size()) { + RCLCPP_ERROR(logger, "joint_goals_update() called with unequal argument sizes"); + } + + for (size_t i = 0; i < goals.first.size(); ++i) { + for (size_t j = 0; j < names.size() && j < values.size(); ++j) { + if (result.first.at(i) == names.at(j)) { + result.second.at(i) = values.at(j); + } + } + } + + return result; +} + +/** + * Update joint goal values in bulk by adding the corresponding difference to the current value + * + * @param goals Initial goal values + * @param names List of names which include the joints that should get updated + * @param diffs Differences to be applied to the previously defined joint names + * @return new joint goals with updated values + */ +inline JointGoals joint_goals_update_diff(const JointGoals &goals, const std::vector &names, + const std::vector &diffs, rclcpp::Logger logger) { + JointGoals result = goals; + + if (names.size() != diffs.size()) { + RCLCPP_ERROR(logger, "joint_goals_update() called with unequal argument sizes"); + } + + for (size_t i = 0; i < goals.first.size(); ++i) { + for (size_t j = 0; j < names.size() && j < diffs.size(); ++j) { + if (result.first.at(i) == names.at(j)) { + result.second.at(i) += diffs.at(j); + } + } + } + + return result; +} + +template +class AbstractIK { + /** + * Initializes the class. This must be called before calculate() is called. + * @param kinematic_model The MoveIt! kinematic model of the robot + */ + virtual void init(moveit::core::RobotModelPtr kinematic_model) = 0; + /** + * Calculate motor joint goals from cartesian positions, i.e. solve the presented inverse kinematics problem + * @param positions the cartesian positions of end points and custom additional information (e.g. support foot) + * @return motor positions + */ + virtual JointGoals calculate(const Positions &positions) = 0; + /** + * Reset the IK to its initial state. + */ + virtual void reset() = 0; +}; +} // namespace bitbots_splines + +#endif // BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_IK_H_ diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_stabilizer.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_stabilizer.hpp similarity index 84% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_stabilizer.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_stabilizer.hpp index 4b54da719..ee736462f 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_stabilizer.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_stabilizer.hpp @@ -5,7 +5,7 @@ namespace bitbots_splines { -template +template class AbstractStabilizer { public: /** @@ -19,8 +19,7 @@ class AbstractStabilizer { * @return A pointer to BioIK Goals that can be passed to the AbstractIK. */ virtual Positions stabilize(const Positions &positions, const rclcpp::Duration &dt) = 0; - }; -} +} // namespace bitbots_splines -#endif //BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_STABILIZER_H_ +#endif // BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_STABILIZER_H_ diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_visualizer.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_visualizer.hpp similarity index 91% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_visualizer.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_visualizer.hpp index 2bc6818bb..bf57bca7d 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_visualizer.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/abstract_visualizer.hpp @@ -1,10 +1,11 @@ #ifndef BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_VISUALIZER_H_ #define BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_VISUALIZER_H_ +#include #include + #include #include -#include namespace bitbots_splines { @@ -17,12 +18,13 @@ class AbstractVisualizer { * @param frame The frame in which the position is given * @return The visualization marker */ - visualization_msgs::msg::Marker getMarker(const tf2::Vector3 &position, const std::string &frame, rclcpp::Node::SharedPtr node) { + visualization_msgs::msg::Marker getMarker(const tf2::Vector3 &position, const std::string &frame, + rclcpp::Node::SharedPtr node) { visualization_msgs::msg::Marker marker; marker.action = visualization_msgs::msg::Marker::ADD; marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.lifetime = rclcpp::Duration::from_nanoseconds(1e9*1000); + marker.lifetime = rclcpp::Duration::from_nanoseconds(1e9 * 1000); marker.frame_locked = false; marker.header.frame_id = frame; marker.header.stamp = node->now(); @@ -45,15 +47,12 @@ class AbstractVisualizer { * @param smoothness The smoothness of the splines * @return The visualization markers */ - visualization_msgs::msg::MarkerArray getPath(bitbots_splines::PoseSpline &spline, - const std::string &frame, - const double smoothness, - rclcpp::Node::SharedPtr node) { - + visualization_msgs::msg::MarkerArray getPath(bitbots_splines::PoseSpline &spline, const std::string &frame, + const double smoothness, rclcpp::Node::SharedPtr node) { visualization_msgs::msg::MarkerArray marker_array; visualization_msgs::msg::Marker base_marker; base_marker.action = visualization_msgs::msg::Marker::ADD; - base_marker.lifetime = rclcpp::Duration::from_nanoseconds(1e9*1000); + base_marker.lifetime = rclcpp::Duration::from_nanoseconds(1e9 * 1000); base_marker.frame_locked = true; base_marker.header.frame_id = frame; base_marker.header.stamp = node->now(); @@ -133,6 +132,6 @@ class AbstractVisualizer { return marker_array; } }; -} +} // namespace bitbots_splines -#endif //BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_VISUALIZER_H_ +#endif // BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_VISUALIZER_H_ diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/combination.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/combination.hpp similarity index 93% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/combination.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/combination.hpp index 8a75346a8..6117834ce 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/combination.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/combination.hpp @@ -15,13 +15,12 @@ namespace bitbots_splines { * Combination * * Implement binomial coefficient - * computation using Pascal Triangle + * computation using Pascal Triangle * and iterate thought all (n choose k) * combinations */ class Combination { public: - /** * Compute the number of possible * combinations for (n choose k) @@ -43,7 +42,6 @@ class Combination { std::vector nextCombination(); private: - /** * Typedefs */ @@ -61,8 +59,8 @@ class Combination { * iteration n and k parameter */ std::vector indexes_; - size_t n_; - size_t k_; + size_t n_ = 0; + size_t k_ = 0; /** * Increment by one the _indexes container @@ -72,7 +70,6 @@ class Combination { bool incrIndexes(size_t i); }; -} +} // namespace bitbots_splines #endif - diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/euler.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/euler.hpp similarity index 78% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/euler.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/euler.hpp index 4749cbf0f..17cf51a61 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/euler.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/euler.hpp @@ -6,9 +6,9 @@ The original files can be found at: #ifndef EULER_H #define EULER_H -#include #include #include +#include namespace bitbots_splines { @@ -41,20 +41,17 @@ enum EulerType { * Return false if given Euler angles range are no valid */ inline bool CheckEulerBounds(const Eigen::Vector3d &angles) { - return - (angles(0) >= -M_PI && angles(0) <= M_PI) && - ((angles(1) > -M_PI / 2.0 && angles(1) < M_PI / 2.0) || - (angles(0) == 0 && angles(2) == 0 && - (angles(1) == -M_PI / 2.0 || angles(1) == M_PI / 2.0))) && - (angles(2) >= 0.0 && angles(2) <= M_PI); + return (angles(0) >= -M_PI && angles(0) <= M_PI) && + ((angles(1) > -M_PI / 2.0 && angles(1) < M_PI / 2.0) || + (angles(0) == 0 && angles(2) == 0 && (angles(1) == -M_PI / 2.0 || angles(1) == M_PI / 2.0))) && + (angles(2) >= 0.0 && angles(2) <= M_PI); } /** * Convert given Euler angles of given * convention type to rotation matrix */ -inline Eigen::Matrix3d EulerToMatrix( - const Eigen::Vector3d &angles, EulerType eulerType) { +inline Eigen::Matrix3d EulerToMatrix(const Eigen::Vector3d &angles, EulerType eulerType) { Eigen::Quaternion quat; switch (eulerType) { case EulerYawPitchRoll: { @@ -62,43 +59,37 @@ inline Eigen::Matrix3d EulerToMatrix( Eigen::AngleAxisd pitchRot(angles(1), Eigen::Vector3d::UnitY()); Eigen::AngleAxisd rollRot(angles(2), Eigen::Vector3d::UnitX()); quat = rollRot * pitchRot * yawRot; - } - break; + } break; case EulerYawRollPitch: { Eigen::AngleAxisd yawRot(angles(0), Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchRot(angles(2), Eigen::Vector3d::UnitY()); Eigen::AngleAxisd rollRot(angles(1), Eigen::Vector3d::UnitX()); quat = pitchRot * rollRot * yawRot; - } - break; + } break; case EulerRollPitchYaw: { Eigen::AngleAxisd yawRot(angles(2), Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchRot(angles(1), Eigen::Vector3d::UnitY()); Eigen::AngleAxisd rollRot(angles(0), Eigen::Vector3d::UnitX()); quat = yawRot * pitchRot * rollRot; - } - break; + } break; case EulerRollYawPitch: { Eigen::AngleAxisd yawRot(angles(1), Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchRot(angles(2), Eigen::Vector3d::UnitY()); Eigen::AngleAxisd rollRot(angles(0), Eigen::Vector3d::UnitX()); quat = pitchRot * yawRot * rollRot; - } - break; + } break; case EulerPitchRollYaw: { Eigen::AngleAxisd yawRot(angles(2), Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchRot(angles(0), Eigen::Vector3d::UnitY()); Eigen::AngleAxisd rollRot(angles(1), Eigen::Vector3d::UnitX()); quat = yawRot * rollRot * pitchRot; - } - break; + } break; case EulerPitchYawRoll: { Eigen::AngleAxisd yawRot(angles(1), Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchRot(angles(0), Eigen::Vector3d::UnitY()); Eigen::AngleAxisd rollRot(angles(2), Eigen::Vector3d::UnitX()); quat = rollRot * yawRot * pitchRot; - } - break; + } break; default: { throw std::logic_error("Euler invalid type"); } @@ -110,34 +101,27 @@ inline Eigen::Matrix3d EulerToMatrix( * Convert the given rotation matrix into * Euler angles of given convention */ -inline Eigen::Vector3d MatrixToEuler( - const Eigen::Matrix3d &mat, EulerType eulerType) { +inline Eigen::Vector3d MatrixToEuler(const Eigen::Matrix3d &mat, EulerType eulerType) { Eigen::Vector3d tmp(0.0, 0.0, 0.0); switch (eulerType) { case EulerYawPitchRoll: { tmp = mat.eulerAngles(0, 1, 2); - } - break; + } break; case EulerYawRollPitch: { tmp = mat.eulerAngles(1, 0, 2); - } - break; + } break; case EulerRollPitchYaw: { tmp = mat.eulerAngles(2, 1, 0); - } - break; + } break; case EulerRollYawPitch: { tmp = mat.eulerAngles(1, 2, 0); - } - break; + } break; case EulerPitchRollYaw: { tmp = mat.eulerAngles(2, 0, 1); - } - break; + } break; case EulerPitchYawRoll: { tmp = mat.eulerAngles(0, 2, 1); - } - break; + } break; } Eigen::Vector3d angles; angles(0) = tmp(2); @@ -148,20 +132,17 @@ inline Eigen::Vector3d MatrixToEuler( /** * Manually convert the given rotation matrix - * to [Roll, Pitch, Yaw] ZYX intrinsic euler + * to [Roll, Pitch, Yaw] ZYX intrinsic euler * angle (Better range than Eigen conversion). */ -inline Eigen::Vector3d MatrixToEulerIntrinsic( - const Eigen::Matrix3d &mat) { - //Eigen euler angles and with better range) +inline Eigen::Vector3d MatrixToEulerIntrinsic(const Eigen::Matrix3d &mat) { + // Eigen euler angles and with better range) Eigen::Vector3d angles; - //Roll + // Roll angles.x() = atan2(mat(2, 1), mat(2, 2)); - //Pitch - angles.y() = atan2(-mat(2, 0), - sqrt(mat(0, 0) * mat(0, 0) - + mat(1, 0) * mat(1, 0))); - //Yaw + // Pitch + angles.y() = atan2(-mat(2, 0), sqrt(mat(0, 0) * mat(0, 0) + mat(1, 0) * mat(1, 0))); + // Yaw angles.z() = atan2(mat(1, 0), mat(0, 0)); return angles; @@ -171,8 +152,7 @@ inline Eigen::Vector3d MatrixToEulerIntrinsic( * Convert given Euler angles in [Roll, Pitch, Yaw] * ZYX intrinsic format to rotation matrix */ -inline Eigen::Matrix3d EulerIntrinsicToMatrix( - const Eigen::Vector3d &angles) { +inline Eigen::Matrix3d EulerIntrinsicToMatrix(const Eigen::Vector3d &angles) { Eigen::AngleAxisd yawRot(angles.z(), Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchRot(angles.y(), Eigen::Vector3d::UnitY()); Eigen::AngleAxisd rollRot(angles.x(), Eigen::Vector3d::UnitX()); @@ -180,7 +160,6 @@ inline Eigen::Matrix3d EulerIntrinsicToMatrix( return quat.matrix(); } -} +} // namespace bitbots_splines #endif - diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/newton_binomial.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/newton_binomial.hpp similarity index 88% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/newton_binomial.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/newton_binomial.hpp index 70ea49c0a..4127843cd 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/newton_binomial.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/newton_binomial.hpp @@ -20,19 +20,16 @@ namespace bitbots_splines { */ class NewtonBinomial { public: - /** * Expand the given formula (x + y)^degree * and return the polynom in x whose coefficient * are computed using binomial coefficient */ - static Polynom expandPolynom( - double y, unsigned int degree); + static Polynom expandPolynom(double y, unsigned int degree); private: }; -} +} // namespace bitbots_splines #endif - diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/polynom.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/polynom.hpp similarity index 94% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/polynom.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/polynom.hpp index 093ae874a..db8e3f7ef 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/polynom.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/polynom.hpp @@ -7,21 +7,20 @@ The original files can be found at: #define BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_POLYNOM_H_ #include -#include #include +#include namespace bitbots_splines { /** * Polynom * - * Simple one dimensional - * polynom class for spline + * Simple one dimensional + * polynom class for spline * generation */ class Polynom { public: - /** * Default and initial degree initialization */ @@ -71,7 +70,6 @@ class Polynom { void shift(double delta); private: - /** * Polynom coefficients */ @@ -83,7 +81,6 @@ class Polynom { */ std::ostream &operator<<(std::ostream &os, const Polynom &p); -} +} // namespace bitbots_splines #endif - diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/pose_spline.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/pose_spline.hpp similarity index 93% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/pose_spline.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/pose_spline.hpp index 48bf40ad1..01ec25ae3 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/pose_spline.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/pose_spline.hpp @@ -3,12 +3,14 @@ #include #include +#include #include #include -#include -#include + #include +#include #include + #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace bitbots_splines { @@ -47,7 +49,6 @@ class PoseSpline { SmoothSpline roll_; SmoothSpline pitch_; SmoothSpline yaw_; - }; -} -#endif //BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_POSE_SPLINE_H_ +} // namespace bitbots_splines +#endif // BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_POSE_SPLINE_H_ diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/position_spline.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/position_spline.hpp similarity index 86% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/position_spline.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/position_spline.hpp index e268804d8..e5c19ce25 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/position_spline.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/position_spline.hpp @@ -4,6 +4,7 @@ #include #include #include + #include namespace bitbots_splines { @@ -24,7 +25,6 @@ class PositionSpline { SmoothSpline x_; SmoothSpline y_; SmoothSpline z_; - }; -} -#endif //BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_POSITION_SPLINE_H_ +} // namespace bitbots_splines +#endif // BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_POSITION_SPLINE_H_ diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.hpp similarity index 82% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.hpp index 013ba4ddd..b6fe8ffab 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/smooth_spline.hpp @@ -18,7 +18,6 @@ namespace bitbots_splines { */ class SmoothSpline : public Spline { public: - /** * Simple point struture */ @@ -33,8 +32,7 @@ class SmoothSpline : public Spline { * Add a new point with its time, position value, * velocity and acceleration */ - void addPoint(double time, double position, - double velocity = 0.0, double acceleration = 0.0); + void addPoint(double time, double position, double velocity = 0.0, double acceleration = 0.0); /** * Access to points container @@ -54,7 +52,6 @@ class SmoothSpline : public Spline { std::string getDebugString(); protected: - /** * Inherit * Load Points @@ -62,7 +59,6 @@ class SmoothSpline : public Spline { void importCallBack() override; private: - /** * Points container */ @@ -72,12 +68,10 @@ class SmoothSpline : public Spline { * Fit a polynom between 0 and t with given * pos, vel and acc initial and final conditions */ - Polynom polynomFit(double t, - double pos_1, double vel_1, double acc_1, - double pos_2, double vel_2, double acc_2) const; + Polynom polynomFit(double t, double pos_1, double vel_1, double acc_1, double pos_2, double vel_2, + double acc_2) const; }; -} +} // namespace bitbots_splines #endif - diff --git a/bitbots_motion/bitbots_splines/include/bitbots_splines/spline_container.h b/bitbots_motion/bitbots_splines/include/bitbots_splines/spline_container.hpp similarity index 79% rename from bitbots_motion/bitbots_splines/include/bitbots_splines/spline_container.h rename to bitbots_motion/bitbots_splines/include/bitbots_splines/spline_container.hpp index c04c2cce7..a83774606 100644 --- a/bitbots_motion/bitbots_splines/include/bitbots_splines/spline_container.h +++ b/bitbots_motion/bitbots_splines/include/bitbots_splines/spline_container.hpp @@ -6,15 +6,17 @@ The original files can be found at: #ifndef BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_SPLINE_CONTAINER_H_ #define BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_SPLINE_CONTAINER_H_ -#include -#include -#include +#include + +#include #include -#include "spline.hpp" +#include #include -#include +#include +#include #include -#include + +#include "spline.hpp" namespace bitbots_splines { @@ -25,27 +27,23 @@ namespace bitbots_splines { * types indexed by string name * Implementation of implort/export from files */ -template +template class SplineContainer { public: - /** * Return the number of contained splines */ - inline size_t size() const { - return container_.size(); - } + inline size_t size() const { return container_.size(); } /** * Add an empty spline with given name. * Variadic arguments allow to pass parameters to * spline constructor. */ - template + template inline void add(const std::string &name, Args... args) { if (container_.count(name) != 0) { - throw std::logic_error( - "SplineContainer spline already added"); + throw std::logic_error("SplineContainer spline already added"); } container_[name] = T(args...); } @@ -54,24 +52,20 @@ class SplineContainer { * Return true if given spline * name is contained */ - inline bool exist(const std::string &name) const { - return container_.count(name) > 0; - } + inline bool exist(const std::string &name) const { return container_.count(name) > 0; } /** * Access to given named spline */ inline const T &get(const std::string &name) const { if (container_.count(name) == 0) { - throw std::logic_error( - "SplineContainer invalid name: " + name); + throw std::logic_error("SplineContainer invalid name: " + name); } return container_.at(name); } inline T &get(const std::string &name) { if (container_.count(name) == 0) { - throw std::logic_error( - "SplineContainer invalid name: " + name); + throw std::logic_error("SplineContainer invalid name: " + name); } return container_.at(name); } @@ -79,12 +73,8 @@ class SplineContainer { /** * Access to internal map container */ - const std::map &get() const { - return container_; - } - std::map &get() { - return container_; - } + const std::map &get() const { return container_; } + std::map &get() { return container_; } /** * Returns all time points where a point in any spline exists. @@ -99,7 +89,7 @@ class SplineContainer { times.insert(point.time); } } - //insert set into vector + // insert set into vector times_sorted.insert(times_sorted.end(), times.begin(), times.end()); std::sort(times_sorted.begin(), times_sorted.end()); return times_sorted; @@ -149,9 +139,7 @@ class SplineContainer { std::ofstream file(file_name); if (!file.is_open()) { - throw std::runtime_error( - "SplineContainer unable to write file: " - + file_name); + throw std::runtime_error("SplineContainer unable to write file: " + file_name); } for (const auto &sp : container_) { @@ -164,42 +152,38 @@ class SplineContainer { void importData(const std::string &file_name) { std::ifstream file(file_name); if (!file.is_open()) { - throw std::runtime_error( - "SplineContainer unable to read file: " - + file_name); + throw std::runtime_error("SplineContainer unable to read file: " + file_name); } - bool is_parse_error; + bool is_parse_error = true; while (file.good()) { is_parse_error = true; - //Skip name delimitor + // Skip name delimitor if (file.peek() != '\'') break; file.ignore(); if (!file.good()) break; - //Parse spline name + // Parse spline name char name[256]; file.getline(name, 256, '\''); - //Import founded spline + // Import founded spline add(std::string(name)); if (!file.good()) break; container_.at(std::string(name)).importData(file); is_parse_error = false; - //Skip end line + // Skip end line while (file.peek() == ' ' || file.peek() == '\n') { if (!file.good()) break; file.ignore(); } } if (is_parse_error) { - throw std::logic_error( - "SplineContainer invalid input format"); + throw std::logic_error("SplineContainer invalid input format"); } file.close(); } private: - /** * Spline container indexed * by their name @@ -207,7 +191,6 @@ class SplineContainer { std::map container_; }; -} +} // namespace bitbots_splines #endif - diff --git a/bitbots_motion/bitbots_splines/src/Spline/polynom.cpp b/bitbots_motion/bitbots_splines/src/Spline/polynom.cpp index ebb9efd5b..691dbcc7d 100644 --- a/bitbots_motion/bitbots_splines/src/Spline/polynom.cpp +++ b/bitbots_motion/bitbots_splines/src/Spline/polynom.cpp @@ -3,9 +3,9 @@ This code is largely based on the original code by Quentin "Leph" Rouxel and Tea The original files can be found at: https://github.com/Rhoban/model/ */ -#include "bitbots_splines/polynom.h" +#include "bitbots_splines/polynom.hpp" -#include "bitbots_splines/newton_binomial.h" +#include "bitbots_splines/newton_binomial.hpp" namespace bitbots_splines { diff --git a/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp b/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp index 56d967eec..3e8392cb7 100644 --- a/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp +++ b/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp @@ -1,4 +1,4 @@ -#include "bitbots_splines/pose_spline.h" +#include "bitbots_splines/pose_spline.hpp" namespace bitbots_splines { @@ -106,4 +106,4 @@ std::string PoseSpline::getDebugString() { return output; } -} // namespace bitbots_splines \ No newline at end of file +} // namespace bitbots_splines diff --git a/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp b/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp index cff76ebdc..c979e9c6f 100644 --- a/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp +++ b/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp @@ -1,4 +1,4 @@ -#include "bitbots_splines/position_spline.h" +#include "bitbots_splines/position_spline.hpp" namespace bitbots_splines { diff --git a/bitbots_motion/bitbots_splines/src/Spline/smooth_spline.cpp b/bitbots_motion/bitbots_splines/src/Spline/smooth_spline.cpp index 7eb2681c5..682eaa761 100644 --- a/bitbots_motion/bitbots_splines/src/Spline/smooth_spline.cpp +++ b/bitbots_motion/bitbots_splines/src/Spline/smooth_spline.cpp @@ -3,7 +3,7 @@ This code is largely based on the original code by Quentin "Leph" Rouxel and Tea The original files can be found at: https://github.com/Rhoban/model/ */ -#include "bitbots_splines/smooth_spline.h" +#include "bitbots_splines/smooth_spline.hpp" #include diff --git a/bitbots_motion/bitbots_splines/src/Utils/combination.cpp b/bitbots_motion/bitbots_splines/src/Utils/combination.cpp index 9b4554124..eb985e418 100644 --- a/bitbots_motion/bitbots_splines/src/Utils/combination.cpp +++ b/bitbots_motion/bitbots_splines/src/Utils/combination.cpp @@ -3,7 +3,7 @@ This code is largely based on the original code by Quentin "Leph" Rouxel and Tea The original files can be found at: https://github.com/Rhoban/model/ */ -#include "bitbots_splines/combination.h" +#include "bitbots_splines/combination.hpp" #include #include diff --git a/bitbots_motion/bitbots_splines/src/Utils/newton_binomial.cpp b/bitbots_motion/bitbots_splines/src/Utils/newton_binomial.cpp index 7b85712de..5c8dcc641 100644 --- a/bitbots_motion/bitbots_splines/src/Utils/newton_binomial.cpp +++ b/bitbots_motion/bitbots_splines/src/Utils/newton_binomial.cpp @@ -3,7 +3,7 @@ This code is largely based on the original code by Quentin "Leph" Rouxel and Tea The original files can be found at: https://github.com/Rhoban/model/ */ -#include "bitbots_splines/newton_binomial.h" +#include "bitbots_splines/newton_binomial.hpp" #include