wrench_planner.hpp

Go to the documentation of this file.

 
#ifndef _WRENCH_PLANNER_HPP
#define _WRENCH_PLANNER_HPP
 
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <localization_msgs/msg/odometry.hpp>
#include <p_pid_controller/p_pid_controller.hpp>
#include <p_pid_controller_msgs/msg/targets.hpp>
#include <planner_msgs/msg/wrench_plan.hpp>
#include <rclcpp/rclcpp.hpp>
 
namespace planner::wrench_planner
{
 
class WrenchPlanner : public rclcpp::Node
{
private:
  rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr pub_;
  rclcpp::Publisher<p_pid_controller_msgs::msg::Targets>::SharedPtr pub_target_;
  rclcpp::Subscription<planner_msgs::msg::WrenchPlan>::SharedPtr sub_;
  rclcpp::Subscription<localization_msgs::msg::Odometry>::SharedPtr sub_odom_;
 
  planner_msgs::msg::WrenchPlan::SharedPtr goal_current_odom_;
  localization_msgs::msg::Odometry::SharedPtr current_odom_;
 
  std::shared_ptr<controller::P_PIDController> p_pid_ctrl_;
 
  uint8_t pre_z_mode_ = 0;
  bool odom_updated_ = false;
 
  void _update_wrench();
 
  void goalCurrentOdomCallback(const planner_msgs::msg::WrenchPlan::SharedPtr msg);
  void odomCallback(const localization_msgs::msg::Odometry::SharedPtr msg);
  bool has_velocity(const planner_msgs::msg::WrenchPlan::SharedPtr msg);
 
public:
  explicit WrenchPlanner(const rclcpp::NodeOptions & options);
};
 
}  // namespace planner::wrench_planner
 
#endif