增加启动脚本，删除无用代码
This commit is contained in:
parent
6fba6f194d
commit
80fca8524c
@ -120,37 +120,6 @@ void signal_handler(int signal) {
|
||||
}
|
||||
}
|
||||
|
||||
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,
|
||||
@ -192,53 +161,6 @@ private:
|
||||
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 {
|
||||
@ -248,7 +170,6 @@ int main(int argc, char **argv) {
|
||||
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";
|
||||
|
||||
@ -256,7 +177,6 @@ int main(int argc, char **argv) {
|
||||
std::cerr
|
||||
<< "Usage: " << argv[0]
|
||||
<< " <leader_urdf_path> <follower_urdf_path> [arm_side] [leader_can] [follower_can]"
|
||||
" [sensor_can]"
|
||||
<< std::endl;
|
||||
return 1;
|
||||
}
|
||||
@ -277,7 +197,6 @@ int main(int argc, char **argv) {
|
||||
|
||||
// Optional: CAN interfaces
|
||||
if (argc >= 6) {
|
||||
leader_can_interface = argv[4];
|
||||
follower_can_interface = argv[5];
|
||||
}
|
||||
|
||||
@ -304,7 +223,6 @@ int main(int argc, char **argv) {
|
||||
// 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;
|
||||
@ -356,14 +274,6 @@ int main(int argc, char **argv) {
|
||||
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");
|
||||
@ -378,42 +288,26 @@ int main(int argc, char **argv) {
|
||||
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);
|
||||
|
||||
@ -424,24 +318,16 @@ int main(int argc, char **argv) {
|
||||
// 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()) {
|
||||
|
||||
104
script/launch_joint_sensor.sh
Executable file
104
script/launch_joint_sensor.sh
Executable file
@ -0,0 +1,104 @@
|
||||
#!/bin/bash
|
||||
#
|
||||
# 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.
|
||||
|
||||
# ========= Configuration =========
|
||||
ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm
|
||||
LEADER_CAN_IF=$2 # Optional: leader CAN interface
|
||||
FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface
|
||||
ARM_TYPE="v10" # Fixed for now
|
||||
TMPDIR="/tmp/openarm_urdf_gen"
|
||||
|
||||
# Validate arm side
|
||||
if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then
|
||||
echo "[ERROR] Invalid arm_side: $ARM_SIDE"
|
||||
echo "Usage: $0 <arm_side: right_arm|left_arm> [leader_can_if] [follower_can_if]"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Set default CAN interfaces if not provided
|
||||
if [ -z "$LEADER_CAN_IF" ]; then
|
||||
if [ "$ARM_SIDE" = "right_arm" ]; then
|
||||
LEADER_CAN_IF="can0"
|
||||
else
|
||||
LEADER_CAN_IF="can1"
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ -z "$FOLLOWER_CAN_IF" ]; then
|
||||
if [ "$ARM_SIDE" = "right_arm" ]; then
|
||||
FOLLOWER_CAN_IF="can2"
|
||||
else
|
||||
FOLLOWER_CAN_IF="can3"
|
||||
fi
|
||||
fi
|
||||
|
||||
# File paths
|
||||
LEADER_URDF_PATH="$TMPDIR/${ARM_TYPE}_leader.urdf"
|
||||
FOLLOWER_URDF_PATH="$TMPDIR/${ARM_TYPE}_follower.urdf"
|
||||
XACRO_FILE="$ARM_TYPE.urdf.xacro"
|
||||
WS_DIR=~/openarm_ros2_ws
|
||||
XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE"
|
||||
BIN_PATH="$WS_DIR/src/openarm_teleop/build/joint_sensor_control"
|
||||
|
||||
# Check workspace
|
||||
if [ ! -d "$WS_DIR" ]; then
|
||||
echo "[ERROR] Could not find workspace at: $WS_DIR" >&2
|
||||
echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2
|
||||
echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Check openarm_description package
|
||||
if [ ! -d "$WS_DIR/src/openarm_description" ]; then
|
||||
echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2
|
||||
echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Check xacro
|
||||
if [ ! -f "$XACRO_PATH" ]; then
|
||||
echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# ================================
|
||||
|
||||
# Check binary
|
||||
if [ ! -f "$BIN_PATH" ]; then
|
||||
echo "[ERROR] Compiled binary not found at: $BIN_PATH"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Source ROS 2
|
||||
# shellcheck source=/dev/null
|
||||
source "$WS_DIR/install/setup.bash"
|
||||
|
||||
# Generate URDFs
|
||||
echo "[INFO] Generating URDFs using xacro..."
|
||||
mkdir -p "$TMPDIR"
|
||||
if ! xacro "$XACRO_PATH" bimanual:=true -o "$LEADER_URDF_PATH"; then
|
||||
echo "[ERROR] Failed to generate URDFs."
|
||||
exit 1
|
||||
fi
|
||||
cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH"
|
||||
|
||||
# Run binary
|
||||
echo "[INFO] Launching unilateral control..."
|
||||
"$BIN_PATH" "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" "$ARM_SIDE" "$LEADER_CAN_IF" "$FOLLOWER_CAN_IF"
|
||||
|
||||
# Cleanup
|
||||
echo "[INFO] Cleaning up temporary files..."
|
||||
rm -rf "$TMPDIR"
|
||||
Loading…
Reference in New Issue
Block a user