别人克隆后没有 install，source 会直接失败。把展开后的 install 放进仓库，符号链接和指向本机 build 的 egg-link 收成真实文件，Python 入口改走当前环境的解释器，这样不用先 colcon build 也能启动。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,139 @@
|
||||
/**
|
||||
* @file flexiv_dual_hardware_interface.hpp
|
||||
* @brief Hardware interface to a pair of Flexiv robots for ROS 2 control.
|
||||
* @copyright Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_HARDWARE__FLEXIV_DUAL_HARDWARE_INTERFACE_HPP_
|
||||
#define FLEXIV_HARDWARE__FLEXIV_DUAL_HARDWARE_INTERFACE_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
|
||||
// ROS
|
||||
#include <rclcpp/clock.hpp>
|
||||
#include <rclcpp/duration.hpp>
|
||||
#include <rclcpp/macros.hpp>
|
||||
#include <rclcpp/logger.hpp>
|
||||
#include <rclcpp/time.hpp>
|
||||
#include <rclcpp_lifecycle/state.hpp>
|
||||
|
||||
// ros2_control hardware_interface
|
||||
#include <hardware_interface/hardware_info.hpp>
|
||||
#include <hardware_interface/system_interface.hpp>
|
||||
#include <hardware_interface/types/hardware_interface_return_values.hpp>
|
||||
|
||||
#include "flexiv_hardware/visibility_control.h"
|
||||
|
||||
// Flexiv
|
||||
#include "flexiv/drdk/robot_pair.hpp"
|
||||
|
||||
namespace flexiv_hardware {
|
||||
|
||||
enum StoppingInterface
|
||||
{
|
||||
NONE,
|
||||
STOP_POSITION,
|
||||
STOP_VELOCITY,
|
||||
STOP_EFFORT
|
||||
};
|
||||
|
||||
class FlexivDualHardwareInterface : public hardware_interface::SystemInterface
|
||||
{
|
||||
public:
|
||||
RCLCPP_SHARED_PTR_DEFINITIONS(FlexivDualHardwareInterface)
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::CallbackReturn on_init(
|
||||
const hardware_interface::HardwareInfo& info) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::return_type prepare_command_mode_switch(
|
||||
const std::vector<std::string>& start_interfaces,
|
||||
const std::vector<std::string>& stop_interfaces) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::return_type perform_command_mode_switch(
|
||||
const std::vector<std::string>& start_interfaces,
|
||||
const std::vector<std::string>& stop_interfaces) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::CallbackReturn on_activate(
|
||||
const rclcpp_lifecycle::State& previous_state) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::CallbackReturn on_deactivate(
|
||||
const rclcpp_lifecycle::State& previous_state) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::return_type read(
|
||||
const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::return_type write(
|
||||
const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||
|
||||
private:
|
||||
// Flexiv DRDK
|
||||
std::unique_ptr<flexiv::drdk::RobotPair> robot_pair_;
|
||||
|
||||
// RDK control mode for joint position and velocity interfaces
|
||||
flexiv::rdk::Mode rdk_control_mode_;
|
||||
|
||||
// External axis type
|
||||
std::string external_axis_type_ = "";
|
||||
|
||||
// Joint commands
|
||||
std::vector<double> hw_commands_joint_positions_;
|
||||
std::vector<double> hw_commands_joint_velocities_;
|
||||
std::vector<double> hw_commands_joint_efforts_;
|
||||
|
||||
// Joint states
|
||||
std::vector<double> hw_states_joint_positions_;
|
||||
std::vector<double> hw_states_joint_velocities_;
|
||||
std::vector<double> hw_states_joint_efforts_;
|
||||
|
||||
// Robot States
|
||||
flexiv::rdk::RobotStates hw_flexiv_robot_states_left_;
|
||||
flexiv::rdk::RobotStates hw_flexiv_robot_states_right_;
|
||||
flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_left_ = &hw_flexiv_robot_states_left_;
|
||||
flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_right_ = &hw_flexiv_robot_states_right_;
|
||||
|
||||
// GPIO commands and states
|
||||
std::vector<double> hw_commands_gpio_out_;
|
||||
std::vector<double> hw_states_gpio_in_;
|
||||
|
||||
// Current digital output map
|
||||
std::map<unsigned int, bool> current_digital_outputs_left_;
|
||||
std::map<unsigned int, bool> current_digital_outputs_right_;
|
||||
|
||||
// Joint mapping
|
||||
struct JointMap
|
||||
{
|
||||
int robot_index; // 0: Left, 1: Right
|
||||
int dof_index; // Index in the robot's q vector
|
||||
};
|
||||
std::vector<JointMap> joint_map_;
|
||||
|
||||
static rclcpp::Logger getLogger();
|
||||
|
||||
// Control modes
|
||||
bool controllers_initialized_;
|
||||
std::vector<uint> stop_modes_;
|
||||
std::vector<std::string> start_modes_;
|
||||
bool position_controller_running_;
|
||||
bool velocity_controller_running_;
|
||||
bool torque_controller_running_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv_hardware */
|
||||
#endif /* FLEXIV_HARDWARE__FLEXIV_DUAL_HARDWARE_INTERFACE_HPP_ */
|
||||
@@ -0,0 +1,156 @@
|
||||
/**
|
||||
* @file flexiv_hardware_interface.hpp
|
||||
* @brief Hardware interface to Flexiv robots for ROS 2 control. Adapted from
|
||||
* ros2_control_demos/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp
|
||||
* @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved.
|
||||
* @author Flexiv
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
|
||||
#define FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
// ROS
|
||||
#include <rclcpp/clock.hpp>
|
||||
#include <rclcpp/duration.hpp>
|
||||
#include <rclcpp/macros.hpp>
|
||||
#include <rclcpp/logger.hpp>
|
||||
#include <rclcpp/time.hpp>
|
||||
#include <rclcpp_lifecycle/state.hpp>
|
||||
|
||||
// ros2_control hardware_interface
|
||||
#include <hardware_interface/hardware_info.hpp>
|
||||
#include <hardware_interface/system_interface.hpp>
|
||||
#include <hardware_interface/types/hardware_interface_return_values.hpp>
|
||||
|
||||
#include "flexiv_hardware/visibility_control.h"
|
||||
|
||||
// Flexiv
|
||||
#include "flexiv/rdk/robot.hpp"
|
||||
|
||||
namespace flexiv_hardware {
|
||||
|
||||
enum StoppingInterface
|
||||
{
|
||||
NONE,
|
||||
STOP_POSITION,
|
||||
STOP_VELOCITY,
|
||||
STOP_EFFORT
|
||||
};
|
||||
|
||||
class FlexivHardwareInterface : public hardware_interface::SystemInterface
|
||||
{
|
||||
public:
|
||||
RCLCPP_SHARED_PTR_DEFINITIONS(FlexivHardwareInterface)
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::CallbackReturn on_init(
|
||||
const hardware_interface::HardwareInfo& info) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::return_type prepare_command_mode_switch(
|
||||
const std::vector<std::string>& start_interfaces,
|
||||
const std::vector<std::string>& stop_interfaces) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::return_type perform_command_mode_switch(
|
||||
const std::vector<std::string>& start_interfaces,
|
||||
const std::vector<std::string>& stop_interfaces) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::CallbackReturn on_activate(
|
||||
const rclcpp_lifecycle::State& previous_state) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::CallbackReturn on_deactivate(
|
||||
const rclcpp_lifecycle::State& previous_state) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::return_type read(
|
||||
const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||
|
||||
FLEXIV_HARDWARE_PUBLIC
|
||||
hardware_interface::return_type write(
|
||||
const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||
|
||||
private:
|
||||
// Flexiv RDK
|
||||
std::unique_ptr<flexiv::rdk::Robot> robot_;
|
||||
|
||||
// RDK control mode for joint position and velocity interfaces
|
||||
flexiv::rdk::Mode rdk_control_mode_;
|
||||
|
||||
// Joint commands
|
||||
std::vector<double> hw_commands_joint_positions_;
|
||||
std::vector<double> hw_commands_joint_velocities_;
|
||||
std::vector<double> hw_commands_joint_efforts_;
|
||||
|
||||
// Joint states
|
||||
std::vector<double> hw_states_joint_positions_;
|
||||
std::vector<double> hw_states_joint_velocities_;
|
||||
std::vector<double> hw_states_joint_efforts_;
|
||||
|
||||
// Robot States
|
||||
flexiv::rdk::RobotStates hw_flexiv_robot_states_;
|
||||
flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_ = &hw_flexiv_robot_states_;
|
||||
|
||||
// GPIO commands and states
|
||||
std::vector<double> hw_commands_gpio_out_;
|
||||
std::vector<double> hw_states_gpio_in_;
|
||||
|
||||
// Map from RDK joint index to ROS joint index
|
||||
// RDK expects: [ext_axis_1, ..., ext_axis_N, arm_joint_1, ..., arm_joint_7]
|
||||
std::vector<size_t> rdk_to_ros_map_;
|
||||
|
||||
// Current digital output map
|
||||
std::map<unsigned int, bool> current_digital_outputs_;
|
||||
|
||||
static rclcpp::Logger getLogger();
|
||||
|
||||
// Control modes
|
||||
bool controllers_initialized_;
|
||||
std::vector<uint> stop_modes_;
|
||||
std::vector<std::string> start_modes_;
|
||||
bool position_controller_running_;
|
||||
bool velocity_controller_running_;
|
||||
bool torque_controller_running_;
|
||||
|
||||
// ===== 连接存活看门狗（2026-09-28 真机踩到）=====
|
||||
//
|
||||
// 现象：RDK 断连后 `operational()` 会**一直保持 true**，于是 read() 照跑却只
|
||||
// 返回最后一帧缓存（关节状态永久冻结），write() 把命令发进虚空也不抛异常，
|
||||
// 而 rizon_arm_controller 是本地插值、不校验反馈 —— 最终表现为
|
||||
// **「控制器报 Goal reached/success、关节状态纹丝不动、臂一步没动」**。
|
||||
//
|
||||
// 实测：14:53:44 RDK 打印 "Disconnected from the robot, waiting for reconnection"，
|
||||
// 此后 50 分钟里 /joint_states 的 robot_timestamp 推进量恒为 0，而 `op=1` 没变过。
|
||||
// 根因是两台臂出厂 MAC 相同（88:88:88:88:87:88）在同一 L2 段抢转发，链路丢包 40~53%。
|
||||
//
|
||||
// 这里用 connected() + states().timestamp 双重判活：任一失效就返回 ERROR，
|
||||
// 让 ros2_control 把栈停下来报错，而不是静默地"成功"。
|
||||
int64_t last_state_stamp_ns_ = 0; // 上一帧的机器人时间戳
|
||||
int64_t last_state_change_ns_ = 0; // 上一帧时间戳发生变化的本地时刻
|
||||
|
||||
// D435 粗定位的 MoveL 走已经连上的这份 RDK。write() 里一步一步推进，
|
||||
// 不另开线程，也不在这段里往关节接口灌点。请求文件是 /tmp/flexiv_movel_req。
|
||||
enum class VendorMovelPhase { kIdle, kRunning };
|
||||
VendorMovelPhase vendor_movel_phase_ = VendorMovelPhase::kIdle;
|
||||
long vendor_movel_seq_ = -1;
|
||||
long vendor_movel_done_seq_ = -1;
|
||||
std::chrono::steady_clock::time_point vendor_movel_deadline_ {};
|
||||
bool poll_vendor_movel();
|
||||
void finish_vendor_movel(long seq, bool ok, const std::string& detail);
|
||||
};
|
||||
|
||||
} /* namespace flexiv_hardware */
|
||||
#endif /* FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_ */
|
||||
@@ -0,0 +1,35 @@
|
||||
#ifndef FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_
|
||||
#define FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define FLEXIV_HARDWARE_EXPORT __attribute__((dllexport))
|
||||
#define FLEXIV_HARDWARE_IMPORT __attribute__((dllimport))
|
||||
#else
|
||||
#define FLEXIV_HARDWARE_EXPORT __declspec(dllexport)
|
||||
#define FLEXIV_HARDWARE_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef FLEXIV_HARDWARE_BUILDING_DLL
|
||||
#define FLEXIV_HARDWARE_PUBLIC FLEXIV_HARDWARE_EXPORT
|
||||
#else
|
||||
#define FLEXIV_HARDWARE_PUBLIC FLEXIV_HARDWARE_IMPORT
|
||||
#endif
|
||||
#define FLEXIV_HARDWARE_PUBLIC_TYPE FLEXIV_HARDWARE_PUBLIC
|
||||
#define FLEXIV_HARDWARE_LOCAL
|
||||
#else
|
||||
#define FLEXIV_HARDWARE_EXPORT __attribute__((visibility("default")))
|
||||
#define FLEXIV_HARDWARE_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define FLEXIV_HARDWARE_PUBLIC __attribute__((visibility("default")))
|
||||
#define FLEXIV_HARDWARE_LOCAL __attribute__((visibility("hidden")))
|
||||
#else
|
||||
#define FLEXIV_HARDWARE_PUBLIC
|
||||
#define FLEXIV_HARDWARE_LOCAL
|
||||
#endif
|
||||
#define FLEXIV_HARDWARE_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#endif // FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_
|
||||
Reference in New Issue
Block a user