457 lines
18 KiB
C++
457 lines
18 KiB
C++
|
// Copyright 2025 Enactic, Inc.
|
||
|
|
//
|
||
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
||
|
|
// you may not use this file except in compliance with the License.
|
||
|
|
// You may obtain a copy of the License at
|
||
|
|
//
|
||
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
||
|
|
//
|
||
|
|
// Unless required by applicable law or agreed to in writing, software
|
||
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
||
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||
|
|
// See the License for the specific language governing permissions and
|
||
|
|
// limitations under the License.
|
||
|
|
|
||
|
|
#include <atomic>
|
||
|
|
#include <chrono>
|
||
|
|
#include <controller/control.hpp>
|
||
|
|
#include <controller/dynamics.hpp>
|
||
|
|
#include <csignal>
|
||
|
|
#include <filesystem>
|
||
|
|
#include <iostream>
|
||
|
|
#include <openarm/can/socket/openarm.hpp>
|
||
|
|
#include <openarm/damiao_motor/dm_motor_constants.hpp>
|
||
|
|
#include <linux/can.h>
|
||
|
|
#include <linux/can/raw.h>
|
||
|
|
#include <net/if.h>
|
||
|
|
#include <openarm_port/openarm_init.hpp>
|
||
|
|
#include <periodic_timer_thread.hpp>
|
||
|
|
#include <poll.h>
|
||
|
|
#include <robot_state.hpp>
|
||
|
|
#include <sys/ioctl.h>
|
||
|
|
#include <sys/socket.h>
|
||
|
|
#include <thread>
|
||
|
|
#include <unistd.h>
|
||
|
|
#include <yamlloader.hpp>
|
||
|
|
|
||
|
|
std::atomic<bool> keep_running(true);
|
||
|
|
std::atomic<double> sensor_angle_rad(0.0);
|
||
|
|
std::atomic<double> sensor_velocity_rad_s(0.0);
|
||
|
|
std::atomic<bool> sensor_angle_ready(false);
|
||
|
|
|
||
|
|
constexpr canid_t SENSOR_CAN_ID = 0x01; // TODO: replace with your sensor CAN ID
|
||
|
|
constexpr double SENSOR_SCALE = 1e-3; // TODO: replace with your sensor scaling
|
||
|
|
constexpr double SENSOR_OFFSET = 0.0;
|
||
|
|
constexpr double SENSOR_FILTER_ALPHA = 0.05; // 0~1, larger = less smoothing
|
||
|
|
constexpr double SENSOR_VEL_FILTER_ALPHA = 0.2; // 0~1, larger = less smoothing
|
||
|
|
|
||
|
|
int open_can_socket(const std::string &can_interface) {
|
||
|
|
int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
|
||
|
|
if (sock < 0) {
|
||
|
|
throw std::runtime_error("Failed to open CAN socket");
|
||
|
|
}
|
||
|
|
|
||
|
|
struct ifreq ifr;
|
||
|
|
std::snprintf(ifr.ifr_name, IFNAMSIZ, "%s", can_interface.c_str());
|
||
|
|
if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) {
|
||
|
|
close(sock);
|
||
|
|
throw std::runtime_error("Failed to get CAN interface index");
|
||
|
|
}
|
||
|
|
|
||
|
|
struct sockaddr_can addr;
|
||
|
|
std::memset(&addr, 0, sizeof(addr));
|
||
|
|
addr.can_family = AF_CAN;
|
||
|
|
addr.can_ifindex = ifr.ifr_ifindex;
|
||
|
|
|
||
|
|
if (bind(sock, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) < 0) {
|
||
|
|
close(sock);
|
||
|
|
throw std::runtime_error("Failed to bind CAN socket");
|
||
|
|
}
|
||
|
|
|
||
|
|
struct can_filter filter;
|
||
|
|
filter.can_id = SENSOR_CAN_ID;
|
||
|
|
filter.can_mask = CAN_SFF_MASK;
|
||
|
|
if (setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
|
||
|
|
close(sock);
|
||
|
|
throw std::runtime_error("Failed to set CAN filter");
|
||
|
|
}
|
||
|
|
|
||
|
|
return sock;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool read_sensor(int sock, double &angle_rad_out, double &velocity_rad_s_out) {
|
||
|
|
struct pollfd pfd;
|
||
|
|
pfd.fd = sock;
|
||
|
|
pfd.events = POLLIN;
|
||
|
|
int ret = poll(&pfd, 1, 10);
|
||
|
|
if (ret <= 0 || !(pfd.revents & POLLIN)) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
struct can_frame frame;
|
||
|
|
int nbytes = read(sock, &frame, sizeof(frame));
|
||
|
|
if (nbytes != static_cast<int>(sizeof(frame))) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
const canid_t frame_id = frame.can_id & CAN_SFF_MASK;
|
||
|
|
if (frame_id != SENSOR_CAN_ID || frame.can_dlc < 4) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
int32_t rawPos = 0;
|
||
|
|
rawPos |= static_cast<int32_t>(frame.data[1]) << 8;
|
||
|
|
rawPos |= static_cast<int32_t>(frame.data[2]);
|
||
|
|
int16_t rawVel = 0;
|
||
|
|
rawVel |= static_cast<int32_t>(frame.data[3]) << 8;
|
||
|
|
rawVel |= static_cast<int32_t>(frame.data[4]);
|
||
|
|
|
||
|
|
velocity_rad_s_out = (static_cast<double>(rawVel) / 100.0 / 4096.0) * 360.0 * (M_PI / 180.0); // Example conversion, adjust as needed
|
||
|
|
|
||
|
|
angle_rad_out = (static_cast<double>(rawPos) / 4096.0) * 360.0; // Example conversion, adjust as needed
|
||
|
|
angle_rad_out = angle_rad_out * (M_PI / 180.0) - 3.14;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
void signal_handler(int signal) {
|
||
|
|
if (signal == SIGINT) {
|
||
|
|
std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl;
|
||
|
|
keep_running = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
class LeaderArmThread : public PeriodicTimerThread {
|
||
|
|
public:
|
||
|
|
LeaderArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_l,
|
||
|
|
double hz = 500.0)
|
||
|
|
: PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l) {}
|
||
|
|
|
||
|
|
protected:
|
||
|
|
void before_start() override { std::cout << "leader start thread " << std::endl; }
|
||
|
|
|
||
|
|
void after_stop() override { std::cout << "leader stop thread " << std::endl; }
|
||
|
|
|
||
|
|
void on_timer() override {
|
||
|
|
static auto prev_time = std::chrono::steady_clock::now();
|
||
|
|
|
||
|
|
control_l_->unilateral_step();
|
||
|
|
|
||
|
|
auto now = std::chrono::steady_clock::now();
|
||
|
|
|
||
|
|
auto elapsed_us =
|
||
|
|
std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
|
||
|
|
prev_time = now;
|
||
|
|
// robot_state_->arm_state().get_response(0).position; // Example: access joint 0 position
|
||
|
|
|
||
|
|
// std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
std::shared_ptr<RobotSystemState> robot_state_;
|
||
|
|
Control *control_l_;
|
||
|
|
};
|
||
|
|
|
||
|
|
class FollowerArmThread : public PeriodicTimerThread {
|
||
|
|
public:
|
||
|
|
FollowerArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_f,
|
||
|
|
double hz = 500.0)
|
||
|
|
: PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {}
|
||
|
|
|
||
|
|
protected:
|
||
|
|
void before_start() override { std::cout << "follower start thread " << std::endl; }
|
||
|
|
|
||
|
|
void after_stop() override { std::cout << "follower stop thread " << std::endl; }
|
||
|
|
|
||
|
|
void on_timer() override {
|
||
|
|
static auto prev_time = std::chrono::steady_clock::now();
|
||
|
|
if (sensor_angle_ready.load()) {
|
||
|
|
// control_f_->SetArmJointReference(0, 0, 0.01);
|
||
|
|
control_f_->SetArmJointReference(0, sensor_angle_rad.load(),
|
||
|
|
sensor_velocity_rad_s.load());
|
||
|
|
std::cout << "Sensor angle (rad): " << sensor_angle_rad.load() << std::endl;
|
||
|
|
std::cout << "Sensor velocity (rad/s): " << sensor_velocity_rad_s.load() << std::endl;
|
||
|
|
}
|
||
|
|
// control_f_->SetArmJointReference(0, 0.2, 0.01); // Example: set joint 0 to 0 radians
|
||
|
|
|
||
|
|
control_f_->unilateral_step();
|
||
|
|
|
||
|
|
auto now = std::chrono::steady_clock::now();
|
||
|
|
|
||
|
|
auto elapsed_us =
|
||
|
|
std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
|
||
|
|
prev_time = now;
|
||
|
|
// std::cout << "Leader joint 0 position: " << robot_state_->arm_state().get_reference(0).position
|
||
|
|
// << std::endl;
|
||
|
|
// std::cout << "Leader joint 0 velocity: " << robot_state_->arm_state().get_reference(0).velocity
|
||
|
|
// << std::endl;
|
||
|
|
// std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
std::shared_ptr<RobotSystemState> robot_state_;
|
||
|
|
Control *control_f_;
|
||
|
|
};
|
||
|
|
|
||
|
|
class AdminThread : public PeriodicTimerThread {
|
||
|
|
public:
|
||
|
|
AdminThread(std::shared_ptr<RobotSystemState> leader_state,
|
||
|
|
std::shared_ptr<RobotSystemState> follower_state, Control *control_l,
|
||
|
|
Control *control_f, double hz = 500.0)
|
||
|
|
: PeriodicTimerThread(hz),
|
||
|
|
leader_state_(leader_state),
|
||
|
|
follower_state_(follower_state),
|
||
|
|
control_l_(control_l),
|
||
|
|
control_f_(control_f) {}
|
||
|
|
|
||
|
|
protected:
|
||
|
|
void before_start() override { std::cout << "admin start thread " << std::endl; }
|
||
|
|
|
||
|
|
void after_stop() override { std::cout << "admin stop thread " << std::endl; }
|
||
|
|
|
||
|
|
void on_timer() override {
|
||
|
|
static auto prev_time = std::chrono::steady_clock::now();
|
||
|
|
auto now = std::chrono::steady_clock::now();
|
||
|
|
|
||
|
|
// get response
|
||
|
|
auto leader_arm_resp = leader_state_->arm_state().get_all_responses();
|
||
|
|
auto follower_arm_resp = follower_state_->arm_state().get_all_responses();
|
||
|
|
|
||
|
|
auto leader_hand_resp = leader_state_->hand_state().get_all_responses();
|
||
|
|
auto follower_hand_resp = follower_state_->hand_state().get_all_responses();
|
||
|
|
|
||
|
|
// set referense
|
||
|
|
leader_state_->arm_state().set_all_references(follower_arm_resp);
|
||
|
|
leader_state_->hand_state().set_all_references(follower_hand_resp);
|
||
|
|
|
||
|
|
follower_state_->arm_state().set_all_references(leader_arm_resp);
|
||
|
|
follower_state_->hand_state().set_all_references(leader_hand_resp);
|
||
|
|
|
||
|
|
auto elapsed_us =
|
||
|
|
std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
|
||
|
|
prev_time = now;
|
||
|
|
|
||
|
|
// std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
std::shared_ptr<RobotSystemState> leader_state_;
|
||
|
|
std::shared_ptr<RobotSystemState> follower_state_;
|
||
|
|
Control *control_l_;
|
||
|
|
Control *control_f_;
|
||
|
|
};
|
||
|
|
|
||
|
|
int main(int argc, char **argv) {
|
||
|
|
try {
|
||
|
|
std::signal(SIGINT, signal_handler);
|
||
|
|
|
||
|
|
// default configration
|
||
|
|
std::string arm_side = "right_arm";
|
||
|
|
std::string leader_urdf_path;
|
||
|
|
std::string follower_urdf_path;
|
||
|
|
std::string leader_can_interface = "can0";
|
||
|
|
std::string follower_can_interface = "can2";
|
||
|
|
std::string sensor_can_interface = "can0";
|
||
|
|
|
||
|
|
if (argc < 3) {
|
||
|
|
std::cerr
|
||
|
|
<< "Usage: " << argv[0]
|
||
|
|
<< " <leader_urdf_path> <follower_urdf_path> [arm_side] [leader_can] [follower_can]"
|
||
|
|
" [sensor_can]"
|
||
|
|
<< std::endl;
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Required: URDF paths
|
||
|
|
leader_urdf_path = argv[1];
|
||
|
|
follower_urdf_path = argv[2];
|
||
|
|
|
||
|
|
// Optional: arm_side
|
||
|
|
if (argc >= 4) {
|
||
|
|
arm_side = argv[3];
|
||
|
|
if (arm_side != "left_arm" && arm_side != "right_arm") {
|
||
|
|
std::cerr << "[ERROR] Invalid arm_side: " << arm_side
|
||
|
|
<< ". Must be 'left_arm' or 'right_arm'." << std::endl;
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Optional: CAN interfaces
|
||
|
|
if (argc >= 6) {
|
||
|
|
leader_can_interface = argv[4];
|
||
|
|
follower_can_interface = argv[5];
|
||
|
|
}
|
||
|
|
|
||
|
|
// Optional: sensor CAN interface
|
||
|
|
if (argc >= 7) {
|
||
|
|
sensor_can_interface = argv[6];
|
||
|
|
}
|
||
|
|
|
||
|
|
// URDF file existence check
|
||
|
|
if (!std::filesystem::exists(leader_urdf_path)) {
|
||
|
|
std::cerr << "[ERROR] Leader URDF not found: " << leader_urdf_path << std::endl;
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
if (!std::filesystem::exists(follower_urdf_path)) {
|
||
|
|
std::cerr << "[ERROR] Follower URDF not found: " << follower_urdf_path << std::endl;
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Setup dynamics
|
||
|
|
std::string root_link = "openarm_body_link0";
|
||
|
|
std::string leaf_link =
|
||
|
|
(arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand";
|
||
|
|
|
||
|
|
// Output confirmation
|
||
|
|
std::cout << "=== OpenArm Unilateral Control ===" << std::endl;
|
||
|
|
std::cout << "Arm side : " << arm_side << std::endl;
|
||
|
|
std::cout << "Leader CAN : " << leader_can_interface << std::endl;
|
||
|
|
std::cout << "Follower CAN : " << follower_can_interface << std::endl;
|
||
|
|
std::cout << "Leader URDF path : " << leader_urdf_path << std::endl;
|
||
|
|
std::cout << "Follower URDF path: " << follower_urdf_path << std::endl;
|
||
|
|
std::cout << "Root link : " << root_link << std::endl;
|
||
|
|
std::cout << "Leaf link : " << leaf_link << std::endl;
|
||
|
|
std::cout << "Sensor CAN : " << sensor_can_interface << std::endl;
|
||
|
|
|
||
|
|
int sensor_can_socket = open_can_socket(sensor_can_interface);
|
||
|
|
std::thread can_read_thread([&]() {
|
||
|
|
double filtered_angle = 0.0;
|
||
|
|
bool filter_initialized = false;
|
||
|
|
double filtered_velocity = 0.0;
|
||
|
|
double prev_angle = 0.0;
|
||
|
|
auto prev_time = std::chrono::steady_clock::now();
|
||
|
|
double velocity = 0.0;
|
||
|
|
while (keep_running) {
|
||
|
|
double angle = 0.0;
|
||
|
|
if (read_sensor(sensor_can_socket, angle, velocity)) {
|
||
|
|
auto now = std::chrono::steady_clock::now();
|
||
|
|
double dt =
|
||
|
|
std::chrono::duration_cast<std::chrono::duration<double>>(now - prev_time)
|
||
|
|
.count();
|
||
|
|
// std::cout << "dt: " << dt << " s, raw angle: " << angle << " rad" << std::endl;
|
||
|
|
prev_time = now;
|
||
|
|
|
||
|
|
if (!filter_initialized) {
|
||
|
|
filtered_angle = angle;
|
||
|
|
filter_initialized = true;
|
||
|
|
prev_angle = angle;
|
||
|
|
filtered_velocity = 0.0;
|
||
|
|
} else {
|
||
|
|
filtered_angle = SENSOR_FILTER_ALPHA * angle +
|
||
|
|
(1.0 - SENSOR_FILTER_ALPHA) * filtered_angle;
|
||
|
|
|
||
|
|
if (dt > 1e-6) {
|
||
|
|
filtered_velocity = SENSOR_VEL_FILTER_ALPHA * velocity +
|
||
|
|
(1.0 - SENSOR_VEL_FILTER_ALPHA) *
|
||
|
|
filtered_velocity;
|
||
|
|
}
|
||
|
|
prev_angle = angle;
|
||
|
|
}
|
||
|
|
sensor_angle_rad.store(filtered_angle);
|
||
|
|
sensor_velocity_rad_s.store(filtered_velocity);
|
||
|
|
sensor_angle_ready.store(true);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
});
|
||
|
|
|
||
|
|
YamlLoader leader_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/leader.yaml");
|
||
|
|
YamlLoader follower_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/follower.yaml");
|
||
|
|
|
||
|
|
// Leader parameters
|
||
|
|
std::vector<double> leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp");
|
||
|
|
std::vector<double> leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd");
|
||
|
|
std::vector<double> leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc");
|
||
|
|
std::vector<double> leader_k = leader_loader.get_vector("LeaderArmParam", "k");
|
||
|
|
std::vector<double> leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv");
|
||
|
|
std::vector<double> leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo");
|
||
|
|
|
||
|
|
// Follower parameters
|
||
|
|
std::vector<double> follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp");
|
||
|
|
std::vector<double> follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd");
|
||
|
|
std::vector<double> follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc");
|
||
|
|
std::vector<double> follower_k = follower_loader.get_vector("FollowerArmParam", "k");
|
||
|
|
std::vector<double> follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv");
|
||
|
|
std::vector<double> follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo");
|
||
|
|
|
||
|
|
Dynamics *leader_arm_dynamics = new Dynamics(leader_urdf_path, root_link, leaf_link);
|
||
|
|
leader_arm_dynamics->Init();
|
||
|
|
|
||
|
|
Dynamics *follower_arm_dynamics = new Dynamics(follower_urdf_path, root_link, leaf_link);
|
||
|
|
follower_arm_dynamics->Init();
|
||
|
|
|
||
|
|
std::cout << "=== Initializing Leader OpenArm ===" << std::endl;
|
||
|
|
openarm::can::socket::OpenArm *leader_openarm =
|
||
|
|
openarm_init::OpenArmInitializer::initialize_openarm(leader_can_interface, true);
|
||
|
|
|
||
|
|
std::cout << "=== Initializing Follower OpenArm ===" << std::endl;
|
||
|
|
openarm::can::socket::OpenArm *follower_openarm =
|
||
|
|
openarm_init::OpenArmInitializer::initialize_openarm(follower_can_interface, true);
|
||
|
|
|
||
|
|
size_t leader_arm_motor_num = leader_openarm->get_arm().get_motors().size();
|
||
|
|
size_t follower_arm_motor_num = follower_openarm->get_arm().get_motors().size();
|
||
|
|
size_t leader_hand_motor_num = leader_openarm->get_gripper().get_motors().size();
|
||
|
|
size_t follower_hand_motor_num = follower_openarm->get_gripper().get_motors().size();
|
||
|
|
|
||
|
|
std::cout << "leader arm motor num : " << leader_arm_motor_num << std::endl;
|
||
|
|
std::cout << "follower arm motor num : " << follower_arm_motor_num << std::endl;
|
||
|
|
std::cout << "leader hand motor num : " << leader_hand_motor_num << std::endl;
|
||
|
|
std::cout << "follower hand motor num : " << follower_hand_motor_num << std::endl;
|
||
|
|
|
||
|
|
// Declare robot_state (Joint and motor counts are assumed to be equal)
|
||
|
|
std::shared_ptr<RobotSystemState> leader_state =
|
||
|
|
std::make_shared<RobotSystemState>(leader_arm_motor_num, leader_hand_motor_num);
|
||
|
|
|
||
|
|
std::shared_ptr<RobotSystemState> follower_state =
|
||
|
|
std::make_shared<RobotSystemState>(follower_arm_motor_num, follower_hand_motor_num);
|
||
|
|
|
||
|
|
Control *control_leader = new Control(
|
||
|
|
leader_openarm, leader_arm_dynamics, follower_arm_dynamics, leader_state,
|
||
|
|
1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num);
|
||
|
|
Control *control_follower =
|
||
|
|
new Control(follower_openarm, leader_arm_dynamics, follower_arm_dynamics,
|
||
|
|
follower_state, 1.0 / FREQUENCY, ROLE_FOLLOWER, arm_side,
|
||
|
|
follower_arm_motor_num, follower_hand_motor_num);
|
||
|
|
|
||
|
|
control_leader->SetParameter(leader_kp, leader_kd, leader_Fc, leader_k, leader_Fv,
|
||
|
|
leader_Fo);
|
||
|
|
|
||
|
|
control_follower->SetParameter(follower_kp, follower_kd, follower_Fc, follower_k,
|
||
|
|
follower_Fv, follower_Fo);
|
||
|
|
|
||
|
|
// set home postion
|
||
|
|
// std::thread thread_l(&Control::AdjustPosition, control_leader);
|
||
|
|
// std::thread thread_f(&Control::AdjustPosition, control_follower);
|
||
|
|
// thread_l.join();
|
||
|
|
// thread_f.join();
|
||
|
|
|
||
|
|
// Start control process
|
||
|
|
LeaderArmThread leader_thread(leader_state, control_leader, FREQUENCY);
|
||
|
|
FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY);
|
||
|
|
AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower,
|
||
|
|
FREQUENCY);
|
||
|
|
|
||
|
|
leader_thread.start_thread();
|
||
|
|
follower_thread.start_thread();
|
||
|
|
// admin_thread.start_thread();
|
||
|
|
|
||
|
|
while (keep_running) {
|
||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||
|
|
}
|
||
|
|
|
||
|
|
leader_thread.stop_thread();
|
||
|
|
follower_thread.stop_thread();
|
||
|
|
admin_thread.stop_thread();
|
||
|
|
|
||
|
|
leader_openarm->disable_all();
|
||
|
|
follower_openarm->disable_all();
|
||
|
|
|
||
|
|
if (can_read_thread.joinable()) {
|
||
|
|
can_read_thread.join();
|
||
|
|
}
|
||
|
|
close(sensor_can_socket);
|
||
|
|
|
||
|
|
} catch (const std::exception &e) {
|
||
|
|
std::cerr << e.what() << '\n';
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|