Initial commit: extract M20 sim2real into ROS1 invicta project
Source: DeepRoboticsLab/sdk_deploy (BSD-3-Clause), M20 sim2real part only. Packages: - invicta_msgs: JointCommandArray, MotionCommand, MotionState; standard messages (sensor_msgs/JointState, sensor_msgs/Imu) elsewhere; shared motion value defs. - invicta_deploy: state machine (idle/standup/rl/joint-damping/lie-down), ONNX policy with per-gait models, auto RL entry after standup with readiness checks, safety loop, gait->model table; no protocol/network code. - invicta_remote: vendor basic_server protocol (UDP + JSON, proto.md V1.2.0), header/MsgId, 0xE0xx error codes, motion/usage-mode/gait translation, telemetry 1002/3,4,5,6 (2/10/2/2 Hz); publishes /motion_command, subscribes /motion_state. - invicta_hardware: joint/IMU bridge skeleton (SocketCAN I/O is TODO), publishes sensor_msgs/JointState + sensor_msgs/Imu, consumes /joint_commands. Verified on ROS Noetic (Ubuntu 20.04) in docker: clean catkin_make, 200 Hz joint state / 50 Hz RL commands / 20 Hz state report, protocol conformance (MotionParam, gait preconditions, error codes), telemetry push, and end-to-end switching of two policy models with a real remote controller.
This commit is contained in:
@@ -0,0 +1,39 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(invicta_hardware)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
if(NOT CMAKE_BUILD_TYPE)
|
||||
set(CMAKE_BUILD_TYPE Release)
|
||||
endif()
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
invicta_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs invicta_msgs
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_executable(invicta_hardware_node
|
||||
src/invicta_hardware_node.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(invicta_hardware_node ${catkin_LIBRARIES})
|
||||
|
||||
install(TARGETS invicta_hardware_node
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
install(DIRECTORY launch config
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
@@ -0,0 +1,55 @@
|
||||
# invicta_hardware
|
||||
|
||||
关节/IMU 数据源与关节指令执行端。**当前是骨架**：结构、话题、参数都齐了，只有 `ReadHardware()` / `WriteHardware()` 是待实现的 TODO。
|
||||
|
||||
## 它负责什么
|
||||
|
||||
以 `publish_rate_hz`（默认 200 Hz）为一个周期，循环做三件事：
|
||||
|
||||
1. `ReadHardware()` — 从电机/IMU 读回状态（TODO）
|
||||
2. `PublishState()` — 发布 `sensor_msgs/JointState` 与 `sensor_msgs/Imu`（已实现）
|
||||
3. `WriteHardware()` — 把最新指令下发到电机（TODO，含长度与有限性校验、控制字处理）
|
||||
|
||||
| 话题 | 方向 | 消息 |
|
||||
|---|---|---|
|
||||
| `/joint_states` | 发布 | `sensor_msgs/JointState` |
|
||||
| `/imu/data` | 发布 | `sensor_msgs/Imu` |
|
||||
| `/joint_commands` | 订阅 | `invicta_msgs/JointCommandArray` |
|
||||
|
||||
## 约定（最重要的部分）
|
||||
|
||||
**硬件约定（方向、零点、单位、CAN 帧格式）在本节点内消化，话题上流动的必须是 policy 约定下的量**：
|
||||
|
||||
- 关节角 `[rad]`、角速度 `[rad/s]`、力矩 `[Nm]`，按 `joint_names` 的固定顺序。
|
||||
- IMU：姿态四元数（**弧度**）、角速度 `[rad/s]`（机体系）、线加速度 `[m/s²]`（机体系）。
|
||||
- ⚠️ 上游 M20 的 `/IMU_DATA` 里 `rpy` 是**度**，不要照搬；本工程统一弧度。四元数能避免单位歧义，这也是选 `sensor_msgs/Imu` 的原因之一。
|
||||
- 姿态确实拿不到时，必须把 `orientation_covariance[0]` 置 `-1`（而不是发单位四元数假装有效），deploy 侧会据此丢弃该帧。
|
||||
|
||||
方向/零点换算参考（M20 原车，角度制）：`joint_dir = [1,1,-1,1, 1,-1,1,-1, -1,1,-1,1, -1,-1,1,-1]`，`joint_offset_deg = [-25,-131,160,0, 25,-131,160,0, -25,131,-160,0, 25,131,-160,0]`。新机器人用自己的值。
|
||||
|
||||
**同一套换算只能在一边施加**：本节点做了，`invicta_deploy` 的 `calibration/*` 就保持恒等（默认如此）；反之则要把参数填上并打开 `auto_calibrate`。
|
||||
|
||||
## 待实现清单
|
||||
|
||||
- [ ] `ReadHardware()`：SocketCAN（或你的总线）读回 16 关节的 position/velocity/effort + IMU，换算到 policy 约定。读失败返回 `false`，保留上一帧值，**不要发 NaN**。
|
||||
- [ ] `WriteHardware()`：按 `control_word` 落实使能/失能/清错，把目标角/角速度/前馈力矩/`kp`/`kd` 组装成帧下发；`tau = kp*(q_des-q) + kd*(dq_des-dq) + tau_ff` 由驱动器算还是本节点算，取决于关节模组协议，选一种并写清楚。
|
||||
- [ ] 上电顺序与安全：使能前的自检、通信超时（多久没指令就卸力）、过温/过流保护。注意 deploy 在 `idle` 状态也会以 200 Hz 发零增益指令，所以「按控制字判断是否使能」比「按有无指令判断」更稳。
|
||||
- [ ] 关节顺序必须与 `invicta_deploy` 的 `joint_names` 完全一致（默认 16 个 `fl/fr/hl/hr × hipx/hipy/knee/wheel`）。
|
||||
|
||||
## 参数（私有命名空间，`config/invicta_hardware.yaml`）
|
||||
|
||||
| 参数 | 默认 | 说明 |
|
||||
|---|---|---|
|
||||
| `joint_states_topic` / `imu_topic` / `joint_commands_topic` | `joint_states` / `imu/data` / `joint_commands` | 话题名 |
|
||||
| `imu_frame_id` | `base_link` | IMU 坐标系 |
|
||||
| `publish_rate_hz` | 200.0 | 状态发布频率（需 ≥ deploy 的 200 Hz 控制周期） |
|
||||
| `command_queue` | 1 | 指令只取最新一帧 |
|
||||
| `joint_names` | 16 个 | 关节顺序 |
|
||||
|
||||
## 运行
|
||||
|
||||
```bash
|
||||
roslaunch invicta_hardware invicta_hardware.launch
|
||||
```
|
||||
|
||||
骨架状态下会打印告警，并且只发布零位状态、只缓存指令，**不会驱动电机**。
|
||||
@@ -0,0 +1,17 @@
|
||||
# invicta_hardware 参数（私有命名空间 ~）
|
||||
|
||||
# ---- 话题与频率 ----
|
||||
joint_states_topic: joint_states # sensor_msgs/JointState
|
||||
imu_topic: imu/data # sensor_msgs/Imu
|
||||
joint_commands_topic: joint_commands # invicta_msgs/JointCommandArray
|
||||
imu_frame_id: base_link
|
||||
|
||||
publish_rate_hz: 200.0 # 状态发布频率，需 >= RL 侧 200 Hz 控制周期
|
||||
command_queue: 1 # 指令只取最新一帧
|
||||
|
||||
# ---- 关节顺序（必须与 invicta_deploy 的 joint_names 完全一致）----
|
||||
joint_names:
|
||||
[fl_hipx_joint, fl_hipy_joint, fl_knee_joint, fl_wheel_joint,
|
||||
fr_hipx_joint, fr_hipy_joint, fr_knee_joint, fr_wheel_joint,
|
||||
hl_hipx_joint, hl_hipy_joint, hl_knee_joint, hl_wheel_joint,
|
||||
hr_hipx_joint, hr_hipy_joint, hr_knee_joint, hr_wheel_joint]
|
||||
@@ -0,0 +1,109 @@
|
||||
/**
|
||||
* @file hardware_bridge.h
|
||||
* @brief 关节/IMU 数据源与关节指令执行端（骨架，硬件 I/O 待实现）
|
||||
*
|
||||
* 这个节点是「关节状态发布 + 关节命令接收」合一的那一个：
|
||||
* - 以 publish_rate_hz（默认 200 Hz）向 /joint_states 与 /imu/data 发布状态
|
||||
* - 订阅 /joint_commands，缓存最新一帧，交给 writeMotors() 下发
|
||||
*
|
||||
* 约定（重要，见 README）：
|
||||
* 硬件约定（关节方向 dir、零点 offset、单位、CAN 帧格式）全部由本节点负责，
|
||||
* 话题上流动的必须是 policy 约定下的量：关节角 [rad]、角速度 [rad/s]、力矩 [Nm]、
|
||||
* IMU 姿态为四元数且角度为弧度。这样 invicta_deploy 侧的 calibration 参数保持恒等。
|
||||
*
|
||||
* 当前 ReadHardware()/WriteHardware() 是空的 TODO，节点只会：
|
||||
* - 发布全零的关节状态与单位四元数姿态（说明数据源还没接上）
|
||||
* - 缓存指令并打印控制字变化，不会真的驱动电机
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/JointState.h>
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include "invicta_msgs/JointCommandArray.h"
|
||||
|
||||
namespace invicta_hardware {
|
||||
|
||||
class HardwareBridge {
|
||||
public:
|
||||
explicit HardwareBridge(const ros::NodeHandle& pnh);
|
||||
|
||||
/// 主循环：以 publish_rate_hz 周期跑「读硬件 → 发状态 → 下发指令」，阻塞
|
||||
void Run();
|
||||
|
||||
void Stop();
|
||||
|
||||
private:
|
||||
void LoadParams(const ros::NodeHandle& pnh);
|
||||
|
||||
void CommandCallback(const invicta_msgs::JointCommandArray::ConstPtr& msg);
|
||||
|
||||
/// 发布一帧关节状态与 IMU
|
||||
void PublishState();
|
||||
|
||||
/// 控制字变化时的处理（使能/失能/清错），后续在 WriteHardware 中落实
|
||||
void HandleControlWord(uint16_t word);
|
||||
|
||||
/**
|
||||
* @brief 从电机读回状态
|
||||
*
|
||||
* TODO(硬件)：接 SocketCAN 读回 16 个关节的
|
||||
* position[rad] / velocity[rad/s] / effort[Nm]
|
||||
* 并做 raw → policy 约定的换算（方向、零点、单位）。
|
||||
* 同时读回 IMU：姿态四元数（弧度）、角速度[rad/s]（机体系）、线加速度[m/s²]（机体系）。
|
||||
* 注意 M20 原车 /IMU_DATA 的 rpy 是「度」，这里约定统一用弧度，不要照搬。
|
||||
* 读失败时返回 false，本帧沿用上一次的值（不要发 NaN）。
|
||||
*/
|
||||
bool ReadHardware();
|
||||
|
||||
/**
|
||||
* @brief 把最新指令下发给电机
|
||||
*
|
||||
* TODO(硬件)：把 policy 约定的量换算成 raw（零点、方向、单位），
|
||||
* 按 invicta_msgs/control_word.h 的语义设置使能状态，组装 CAN 帧写下去。
|
||||
* torque = kp*(q_des - q) + kd*(dq_des - dq) + tau_ff 由驱动器或本节点计算，
|
||||
* 取决于关节模组协议，二选一并写清楚。
|
||||
*/
|
||||
bool WriteHardware();
|
||||
|
||||
// ---- 参数 ----
|
||||
std::string joint_state_topic_ = "joint_states";
|
||||
std::string imu_topic_ = "imu/data";
|
||||
std::string command_topic_ = "joint_commands";
|
||||
std::string imu_frame_id_ = "base_link";
|
||||
std::vector<std::string> joint_names_;
|
||||
double publish_rate_hz_ = 200.0;
|
||||
int command_queue_ = 1;
|
||||
|
||||
// ---- ROS ----
|
||||
ros::NodeHandle nh_;
|
||||
ros::Publisher joint_state_pub_;
|
||||
ros::Publisher imu_pub_;
|
||||
ros::Subscriber command_sub_;
|
||||
|
||||
// ---- 指令缓存 ----
|
||||
std::mutex cmd_mutex_;
|
||||
invicta_msgs::JointCommandArray latest_cmd_;
|
||||
bool has_cmd_ = false;
|
||||
uint16_t active_control_word_ = 0;
|
||||
|
||||
// ---- 状态缓存（policy 约定）----
|
||||
std::vector<double> joint_position_, joint_velocity_, joint_effort_;
|
||||
double imu_orientation_[4] = {1.0, 0.0, 0.0, 0.0}; ///< w,x,y,z
|
||||
double imu_angular_velocity_[3] = {0.0, 0.0, 0.0};
|
||||
double imu_linear_acceleration_[3] = {0.0, 0.0, 0.0};
|
||||
|
||||
std::atomic<bool> running_{false};
|
||||
bool warned_no_command_ = false;
|
||||
};
|
||||
|
||||
} // namespace invicta_hardware
|
||||
@@ -0,0 +1,6 @@
|
||||
<launch>
|
||||
<!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数在节点的私有命名空间下 -->
|
||||
<node pkg="invicta_hardware" type="invicta_hardware_node" name="invicta_hardware" output="screen">
|
||||
<rosparam command="load" file="$(find invicta_hardware)/config/invicta_hardware.yaml"/>
|
||||
</node>
|
||||
</launch>
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>invicta_hardware</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
关节/IMU 数据源与关节指令执行端（骨架）：发布 sensor_msgs/JointState 与 sensor_msgs/Imu，
|
||||
订阅 invicta_msgs/JointCommandArray，负责硬件约定（方向、零点、单位、CAN）与 policy 约定之间的换算。
|
||||
</description>
|
||||
<maintainer email="todo@example.com">TODO</maintainer>
|
||||
<license>BSD-3-Clause</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>roscpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>invicta_msgs</depend>
|
||||
</package>
|
||||
@@ -0,0 +1,180 @@
|
||||
/**
|
||||
* @file invicta_hardware_node.cpp
|
||||
* @brief 关节/IMU 数据源 + 关节指令执行端（骨架入口）
|
||||
*/
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <thread>
|
||||
|
||||
#include "invicta_hardware/hardware_bridge.h"
|
||||
#include "invicta_msgs/control_word.h"
|
||||
|
||||
namespace invicta_hardware {
|
||||
|
||||
HardwareBridge::HardwareBridge(const ros::NodeHandle& pnh) : nh_() {
|
||||
LoadParams(pnh);
|
||||
joint_position_.assign(joint_names_.size(), 0.0);
|
||||
joint_velocity_.assign(joint_names_.size(), 0.0);
|
||||
joint_effort_.assign(joint_names_.size(), 0.0);
|
||||
|
||||
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>(joint_state_topic_, 1);
|
||||
imu_pub_ = nh_.advertise<sensor_msgs::Imu>(imu_topic_, 1);
|
||||
command_sub_ = nh_.subscribe(command_topic_, command_queue_, &HardwareBridge::CommandCallback, this);
|
||||
|
||||
ROS_WARN("[invicta_hardware] 这是骨架节点：ReadHardware/WriteHardware 尚未实现，");
|
||||
ROS_WARN("[invicta_hardware] 只会发布零位状态并缓存指令，不会驱动电机。实现见 hardware_bridge.h 的 TODO。");
|
||||
ROS_INFO("[invicta_hardware] %s -> %s/%s，订阅 %s @ %.0f Hz", joint_state_topic_.c_str(), imu_topic_.c_str(),
|
||||
imu_frame_id_.c_str(), command_topic_.c_str(), publish_rate_hz_);
|
||||
}
|
||||
|
||||
void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
|
||||
pnh.param<std::string>("joint_states_topic", joint_state_topic_, "joint_states");
|
||||
pnh.param<std::string>("imu_topic", imu_topic_, "imu/data");
|
||||
pnh.param<std::string>("joint_commands_topic", command_topic_, "joint_commands");
|
||||
pnh.param<std::string>("imu_frame_id", imu_frame_id_, "base_link");
|
||||
pnh.param<double>("publish_rate_hz", publish_rate_hz_, 200.0);
|
||||
pnh.param<int>("command_queue", command_queue_, 1);
|
||||
|
||||
joint_names_ = {"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fl_wheel_joint",
|
||||
"fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint", "fr_wheel_joint",
|
||||
"hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hl_wheel_joint",
|
||||
"hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint", "hr_wheel_joint"};
|
||||
pnh.getParam("joint_names", joint_names_);
|
||||
|
||||
if (publish_rate_hz_ <= 0.) {
|
||||
ROS_ERROR("[invicta_hardware] publish_rate_hz 必须 > 0，回退到 200 Hz");
|
||||
publish_rate_hz_ = 200.0;
|
||||
}
|
||||
}
|
||||
|
||||
void HardwareBridge::CommandCallback(const invicta_msgs::JointCommandArray::ConstPtr& msg) {
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
if (msg->control_word != active_control_word_) {
|
||||
HandleControlWord(msg->control_word);
|
||||
active_control_word_ = msg->control_word;
|
||||
}
|
||||
latest_cmd_ = *msg;
|
||||
has_cmd_ = true;
|
||||
}
|
||||
|
||||
void HardwareBridge::HandleControlWord(uint16_t word) {
|
||||
switch (word) {
|
||||
case invicta_msgs::kDisable:
|
||||
ROS_INFO("[invicta_hardware] control_word -> kDisable(1)：清使能，电机自由");
|
||||
break;
|
||||
case invicta_msgs::kEnable:
|
||||
ROS_INFO("[invicta_hardware] control_word -> kEnable(2)：使能并保持当前角");
|
||||
break;
|
||||
case invicta_msgs::kMotorControl:
|
||||
ROS_INFO("[invicta_hardware] control_word -> kMotorControl(3)：进入 PD+前馈控制");
|
||||
break;
|
||||
case invicta_msgs::kErrorReset:
|
||||
ROS_INFO("[invicta_hardware] control_word -> kErrorReset(4)：清驱动错误");
|
||||
break;
|
||||
case invicta_msgs::kGetStatus:
|
||||
ROS_INFO("[invicta_hardware] control_word -> kGetStatus(5)：上报驱动状态");
|
||||
break;
|
||||
default:
|
||||
ROS_WARN("[invicta_hardware] 未知 control_word=%u，忽略", word);
|
||||
break;
|
||||
}
|
||||
// TODO(硬件)：把上面这些语义落到电机使能序列上（上电顺序、清错指令、超时保护）。
|
||||
}
|
||||
|
||||
void HardwareBridge::PublishState() {
|
||||
sensor_msgs::JointState js;
|
||||
js.header.stamp = ros::Time::now();
|
||||
js.name = joint_names_;
|
||||
js.position = joint_position_;
|
||||
js.velocity = joint_velocity_;
|
||||
js.effort = joint_effort_;
|
||||
joint_state_pub_.publish(js);
|
||||
|
||||
sensor_msgs::Imu imu;
|
||||
imu.header.stamp = js.header.stamp;
|
||||
imu.header.frame_id = imu_frame_id_;
|
||||
imu.orientation.w = imu_orientation_[0];
|
||||
imu.orientation.x = imu_orientation_[1];
|
||||
imu.orientation.y = imu_orientation_[2];
|
||||
imu.orientation.z = imu_orientation_[3];
|
||||
imu.angular_velocity.x = imu_angular_velocity_[0];
|
||||
imu.angular_velocity.y = imu_angular_velocity_[1];
|
||||
imu.angular_velocity.z = imu_angular_velocity_[2];
|
||||
imu.linear_acceleration.x = imu_linear_acceleration_[0];
|
||||
imu.linear_acceleration.y = imu_linear_acceleration_[1];
|
||||
imu.linear_acceleration.z = imu_linear_acceleration_[2];
|
||||
// 首元素置 0 表示「该字段可用」；若某字段确实拿不到，必须置 -1，让使用方知道不能信
|
||||
imu.orientation_covariance[0] = 0.;
|
||||
imu.angular_velocity_covariance[0] = 0.;
|
||||
imu.linear_acceleration_covariance[0] = 0.;
|
||||
imu_pub_.publish(imu);
|
||||
}
|
||||
|
||||
bool HardwareBridge::ReadHardware() {
|
||||
// TODO(硬件)：SocketCAN 读回关节与 IMU，换算到 policy 约定后写入
|
||||
// joint_position_ / joint_velocity_ / joint_effort_ / imu_*
|
||||
// 读不到就 return false，保留上一帧值。
|
||||
return false;
|
||||
}
|
||||
|
||||
bool HardwareBridge::WriteHardware() {
|
||||
invicta_msgs::JointCommandArray cmd;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(cmd_mutex_);
|
||||
if (!has_cmd_) {
|
||||
if (!warned_no_command_) {
|
||||
warned_no_command_ = true;
|
||||
ROS_WARN("[invicta_hardware] 还没收到 %s，不下发任何指令", command_topic_.c_str());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
cmd = latest_cmd_;
|
||||
}
|
||||
|
||||
// 上游 M20 的启停只看控制字，这里也先做长度与有限性检查，避免把坏指令写进电机
|
||||
const size_t n = joint_names_.size();
|
||||
if (cmd.position.size() != n || cmd.velocity.size() != n || cmd.effort.size() != n || cmd.kp.size() != n ||
|
||||
cmd.kd.size() != n) {
|
||||
ROS_WARN_THROTTLE(5.0, "[invicta_hardware] 指令数组长度与 %zu 个关节不匹配，丢弃该帧", n);
|
||||
return false;
|
||||
}
|
||||
|
||||
// TODO(硬件)：raw = 反换算(零点, 方向, 单位) 后按控制字下发。
|
||||
return false;
|
||||
}
|
||||
|
||||
void HardwareBridge::Run() {
|
||||
running_ = true;
|
||||
const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<double>(1.0 / publish_rate_hz_));
|
||||
auto next = std::chrono::steady_clock::now();
|
||||
|
||||
while (ros::ok() && running_) {
|
||||
next += period;
|
||||
ReadHardware();
|
||||
PublishState();
|
||||
WriteHardware();
|
||||
std::this_thread::sleep_until(next);
|
||||
}
|
||||
}
|
||||
|
||||
void HardwareBridge::Stop() { running_ = false; }
|
||||
|
||||
} // namespace invicta_hardware
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "invicta_hardware");
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
invicta_hardware::HardwareBridge bridge(pnh);
|
||||
std::thread loop_thread(&invicta_hardware::HardwareBridge::Run, &bridge);
|
||||
|
||||
ros::spin();
|
||||
|
||||
bridge.Stop();
|
||||
if (loop_thread.joinable()) {
|
||||
loop_thread.join();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user