|
|
@@ -0,0 +1,153 @@
|
|
|
+#include <daystar_navigation_msgs/msg/guardian.hpp>
|
|
|
+#include <daystar_navigation_msgs/srv/guardian.hpp>
|
|
|
+#include <geometry_msgs/msg/pose_stamped.hpp>
|
|
|
+#include <geometry_msgs/msg/twist.hpp>
|
|
|
+#include <nav_msgs/msg/odometry.hpp>
|
|
|
+#include <saturn_msgs/msg/common_status.hpp>
|
|
|
+#include <sensor_msgs/msg/battery_state.hpp>
|
|
|
+#include <sensor_msgs/msg/imu.hpp>
|
|
|
+#include <sensor_msgs/msg/joint_state.hpp>
|
|
|
+#include <std_msgs/msg/float32_multi_array.hpp>
|
|
|
+#include <std_msgs/msg/int32.hpp>
|
|
|
+#include <std_msgs/msg/u_int32.hpp>
|
|
|
+#include <std_msgs/msg/u_int8.hpp>
|
|
|
+#include <ysc_robot_msgs/srv/robot_command.hpp>
|
|
|
+#include "common.h"
|
|
|
+#include "config_handler.hpp"
|
|
|
+#include "mc_client_interface.h"
|
|
|
+#include "rclcpp/rclcpp.hpp"
|
|
|
+#include "rclcpp_components/register_node_macro.hpp"
|
|
|
+#include "std_msgs/msg/string.hpp"
|
|
|
+
|
|
|
+namespace saturn_ros2 {
|
|
|
+
|
|
|
+class SaturnController {
|
|
|
+ public:
|
|
|
+ SaturnController(const rclcpp::NodeOptions& options);
|
|
|
+ rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
|
|
|
+
|
|
|
+ private:
|
|
|
+ std::shared_ptr<rclcpp::Node> node_;
|
|
|
+ std::shared_ptr<ConfigHandler> config_handler_;
|
|
|
+
|
|
|
+ rclcpp::TimerBase::SharedPtr main_timer_;
|
|
|
+ rclcpp::TimerBase::SharedPtr imu_timer_;
|
|
|
+ rclcpp::TimerBase::SharedPtr legged_odom_timer_;
|
|
|
+
|
|
|
+ // subscription
|
|
|
+
|
|
|
+ //订阅导航速度指令
|
|
|
+ rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
|
|
|
+
|
|
|
+ //订阅从navigation发出的停障结果，用于遥控器停障
|
|
|
+ rclcpp::Subscription<daystar_navigation_msgs::msg::Guardian>::SharedPtr guardian_sub_;
|
|
|
+
|
|
|
+ //订阅是否处于sub状态
|
|
|
+ rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr task_mode_sub_;
|
|
|
+
|
|
|
+ // publisher
|
|
|
+
|
|
|
+ //六足机器人腿部里程计
|
|
|
+ rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr legged_odom_publisher_;
|
|
|
+
|
|
|
+ //六足机器人IMU数据
|
|
|
+ rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
|
|
|
+
|
|
|
+ //机器人基本状态
|
|
|
+ // 0 -> lie down
|
|
|
+ // 1 -> standingup
|
|
|
+ // 2 -> standup
|
|
|
+ // 3 -> enable forcecontrol
|
|
|
+ // 4 -> motion
|
|
|
+ // 5 -> sitting down
|
|
|
+ // 6 -> fall down
|
|
|
+ // 7 -> null
|
|
|
+ rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr robot_state_publisher_;
|
|
|
+
|
|
|
+ // dock_state(std_msgs::msg::UInt32): 机器人上桩状态
|
|
|
+ // 0 -> idle
|
|
|
+ // 1 -> going to dock
|
|
|
+ // 2 -> stand above dock
|
|
|
+ // 3 -> adjust pose
|
|
|
+ // 4 -> lie on dock
|
|
|
+ // 5 -> standing after charging
|
|
|
+ // 6 -> leaving dock
|
|
|
+ // 7 -> leave dock
|
|
|
+ rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr docking_state_publisher_;
|
|
|
+
|
|
|
+ //控制模式
|
|
|
+ // 0 -> 手持遥控
|
|
|
+ // 1 -> 导航
|
|
|
+ rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr control_mode_publisher_;
|
|
|
+
|
|
|
+ //设置六足机器人场景
|
|
|
+ // 0 -> null
|
|
|
+ // 1 -> 趴下
|
|
|
+ // 2 -> 行走步态
|
|
|
+ // 3 -> 楼梯步态
|
|
|
+ // 4 -> 充电
|
|
|
+ rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr scene_publisher_;
|
|
|
+
|
|
|
+ //电池电量信息,目前只有percentage字段有意义
|
|
|
+ rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_state_publisher_;
|
|
|
+
|
|
|
+ //机器人模式，用于向巡检平台上报
|
|
|
+ // 0 ->空闲（默认）
|
|
|
+ // 1 ->手持遥控
|
|
|
+ // 2 ->任务
|
|
|
+ // 3 ->充电中
|
|
|
+ rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr robot_mode_publisher_;
|
|
|
+
|
|
|
+ //机器人在充电桩充电返回true，否则返回false
|
|
|
+ rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr charging_state_publisher_;
|
|
|
+
|
|
|
+ // services
|
|
|
+ rclcpp::Service<ysc_robot_msgs::srv::RobotCommand>::SharedPtr robot_command_service_;
|
|
|
+
|
|
|
+ //用于导航停障中的服务接口
|
|
|
+ rclcpp::Service<daystar_navigation_msgs::srv::Guardian>::SharedPtr guardian_service_;
|
|
|
+
|
|
|
+ robot_control::common::MC_MOTION_IMU_DATA imu_data_;
|
|
|
+ robot_control::common::JETSON_VISUAL_FOOTHOLD_STATE foothold_state_data_;
|
|
|
+ robot_control::common::ROBOT_COMMON_STATUS common_status_data_;
|
|
|
+
|
|
|
+ bool is_robot_in_idle_state_ = true;
|
|
|
+ long int robot_heartbeat_ = 0;
|
|
|
+ float guardian_velocity_decay_ratio_ = 1.0;
|
|
|
+
|
|
|
+ void init();
|
|
|
+
|
|
|
+ void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr vel_msg);
|
|
|
+
|
|
|
+ void robotCommandCallback(const ysc_robot_msgs::srv::RobotCommand::Request::SharedPtr request,
|
|
|
+ ysc_robot_msgs::srv::RobotCommand::Response::SharedPtr response);
|
|
|
+
|
|
|
+ void guardianCallback(const daystar_navigation_msgs::msg::Guardian::SharedPtr guardian_msg);
|
|
|
+
|
|
|
+ void guardianServiceCallback(
|
|
|
+ const daystar_navigation_msgs::srv::Guardian::Request::SharedPtr request,
|
|
|
+ daystar_navigation_msgs::srv::Guardian::Response::SharedPtr response);
|
|
|
+
|
|
|
+ void taskModeCallback(const std_msgs::msg::Bool::SharedPtr task_mode);
|
|
|
+
|
|
|
+ void mainTimerCallback();
|
|
|
+
|
|
|
+ void imuTimerCallback();
|
|
|
+
|
|
|
+ void leggedOodmTimerCallback();
|
|
|
+
|
|
|
+ void publishRobotState();
|
|
|
+
|
|
|
+ void publishDockingState();
|
|
|
+
|
|
|
+ void publishControlMode();
|
|
|
+
|
|
|
+ void publishScene();
|
|
|
+
|
|
|
+ void publishBatteryState();
|
|
|
+
|
|
|
+ void publishChargingState();
|
|
|
+
|
|
|
+ void publishRobotMode();
|
|
|
+};
|
|
|
+} // namespace saturn_ros2
|
wangep1