62 explicit ServoNode(
const rclcpp::NodeOptions& options);
87 void pauseServo(
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
88 const std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
95 void switchCommandType(
const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request>& request,
96 const std::shared_ptr<moveit_msgs::srv::ServoCommandType::Response>& response);
98 void jointJogCallback(
const control_msgs::msg::JointJog::ConstSharedPtr& msg);
99 void twistCallback(
const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg);
100 void poseCallback(
const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg);
102 std::optional<KinematicState> processJointJogCommand(
const moveit::core::RobotStatePtr& robot_state);
103 std::optional<KinematicState> processTwistCommand(
const moveit::core::RobotStatePtr& robot_state);
104 std::optional<KinematicState> processPoseCommand(
const moveit::core::RobotStatePtr& robot_state);
108 const rclcpp::Node::SharedPtr node_;
109 std::unique_ptr<Servo> servo_;
110 servo::Params servo_params_;
111 planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
114 control_msgs::msg::JointJog latest_joint_jog_;
115 geometry_msgs::msg::TwistStamped latest_twist_;
116 geometry_msgs::msg::PoseStamped latest_pose_;
117 rclcpp::Subscription<control_msgs::msg::JointJog>::SharedPtr joint_jog_subscriber_;
118 rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_subscriber_;
119 rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_subscriber_;
121 rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr multi_array_publisher_;
122 rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_publisher_;
123 rclcpp::Publisher<moveit_msgs::msg::ServoStatus>::SharedPtr status_publisher_;
125 rclcpp::Service<moveit_msgs::srv::ServoCommandType>::SharedPtr switch_command_type_;
126 rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr pause_servo_;
129 std::atomic<bool> stop_servo_;
130 std::atomic<bool> servo_paused_;
131 std::atomic<bool> new_joint_jog_msg_, new_twist_msg_, new_pose_msg_;
134 std::thread servo_loop_thread_;
137 std::deque<KinematicState> joint_cmd_rolling_window_;