114 lines
4.1 KiB
C++
114 lines
4.1 KiB
C++
#ifndef QUADRUPED_MANIPULATOR_CONTROL__MOTION_CONTROL_NODE_HPP_
|
|
#define QUADRUPED_MANIPULATOR_CONTROL__MOTION_CONTROL_NODE_HPP_
|
|
|
|
#include <memory>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <rclcpp/rclcpp.hpp>
|
|
#include <rclcpp_action/rclcpp_action.hpp>
|
|
#include <controller_manager_msgs/srv/list_controllers.hpp>
|
|
#include <controller_manager_msgs/srv/switch_controller.hpp>
|
|
#include <trajectory_msgs/msg/joint_trajectory.hpp>
|
|
#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
|
|
|
|
#include "quadruped_manipulator_control/planning/moveit_planner.hpp"
|
|
#include "quadruped_manipulator_control/action/move_j.hpp"
|
|
#include "quadruped_manipulator_control/action/move_l.hpp"
|
|
#include "quadruped_manipulator_control/action/joint_movement.hpp"
|
|
|
|
namespace quadruped_manipulator_control
|
|
{
|
|
|
|
class MotionControlNode : public rclcpp::Node
|
|
{
|
|
public:
|
|
using MoveJ = quadruped_manipulator_control::action::MoveJ;
|
|
using GoalHandleMoveJ = rclcpp_action::ServerGoalHandle<MoveJ>;
|
|
|
|
using MoveL = quadruped_manipulator_control::action::MoveL;
|
|
using GoalHandleMoveL = rclcpp_action::ServerGoalHandle<MoveL>;
|
|
|
|
using JointMovement = quadruped_manipulator_control::action::JointMovement;
|
|
using GoalHandleJointMovement = rclcpp_action::ServerGoalHandle<JointMovement>;
|
|
|
|
explicit MotionControlNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
|
|
~MotionControlNode() override = default;
|
|
|
|
private:
|
|
// Action服务器
|
|
rclcpp_action::Server<MoveJ>::SharedPtr action_server_movej_;
|
|
rclcpp_action::Server<MoveL>::SharedPtr action_server_movel_;
|
|
rclcpp_action::Server<JointMovement>::SharedPtr action_server_joint_movement_;
|
|
|
|
// 规划器 (仅保留MoveIt)
|
|
std::shared_ptr<MoveItPlanner> moveit_planner_;
|
|
|
|
// 控制器客户端
|
|
rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr list_controllers_client_;
|
|
rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_client_;
|
|
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_publisher_;
|
|
|
|
// 参数
|
|
std::string controller_name_;
|
|
std::string urdf_path_;
|
|
double default_speed_;
|
|
double default_acceleration_;
|
|
|
|
// Action回调
|
|
rclcpp_action::GoalResponse handle_goal_movej(
|
|
const rclcpp_action::GoalUUID& uuid,
|
|
std::shared_ptr<const MoveJ::Goal> goal);
|
|
|
|
rclcpp_action::CancelResponse handle_cancel_movej(
|
|
const std::shared_ptr<GoalHandleMoveJ> goal_handle);
|
|
|
|
void handle_accepted_movej(
|
|
const std::shared_ptr<GoalHandleMoveJ> goal_handle);
|
|
|
|
rclcpp_action::GoalResponse handle_goal_movel(
|
|
const rclcpp_action::GoalUUID& uuid,
|
|
std::shared_ptr<const MoveL::Goal> goal);
|
|
|
|
rclcpp_action::CancelResponse handle_cancel_movel(
|
|
const std::shared_ptr<GoalHandleMoveL> goal_handle);
|
|
|
|
void handle_accepted_movel(
|
|
const std::shared_ptr<GoalHandleMoveL> goal_handle);
|
|
|
|
rclcpp_action::GoalResponse handle_goal_joint_movement(
|
|
const rclcpp_action::GoalUUID& uuid,
|
|
std::shared_ptr<const JointMovement::Goal> goal);
|
|
|
|
rclcpp_action::CancelResponse handle_cancel_joint_movement(
|
|
const std::shared_ptr<GoalHandleJointMovement> goal_handle);
|
|
|
|
void handle_accepted_joint_movement(
|
|
const std::shared_ptr<GoalHandleJointMovement> goal_handle);
|
|
|
|
// 执行函数
|
|
void execute_movej(const std::shared_ptr<GoalHandleMoveJ> goal_handle);
|
|
void execute_movel(const std::shared_ptr<GoalHandleMoveL> goal_handle);
|
|
void execute_joint_movement(const std::shared_ptr<GoalHandleJointMovement> goal_handle);
|
|
|
|
// 辅助函数
|
|
bool switch_controller(const std::string& controller_name);
|
|
bool send_joint_trajectory(
|
|
const std::vector<std::string>& joint_names,
|
|
const std::vector<trajectory_msgs::msg::JointTrajectoryPoint>& points);
|
|
double calculate_trajectory_time(
|
|
const std::vector<double>& start_positions,
|
|
const std::vector<double>& end_positions,
|
|
double max_velocity,
|
|
double max_acceleration);
|
|
|
|
// 从MoveIt获取关节限位
|
|
bool get_joint_limits(
|
|
const std::string& joint_name,
|
|
double& min_position,
|
|
double& max_position);
|
|
};
|
|
|
|
} // namespace quadruped_manipulator_control
|
|
|
|
#endif // QUADRUPED_MANIPULATOR_CONTROL__MOTION_CONTROL_NODE_HPP_
|