diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 13fdd6b..481c22b 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -16,6 +16,7 @@ Guidelines for modifications: ## Contributors * Jijie Huang +* Yufeng Chi (-T.K.-) ## Acknowledgements diff --git a/src/rl_sar/CMakeLists.txt b/src/rl_sar/CMakeLists.txt index 951d4bd..05627af 100644 --- a/src/rl_sar/CMakeLists.txt +++ b/src/rl_sar/CMakeLists.txt @@ -18,7 +18,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") find_package(gazebo REQUIRED) find_package(ament_cmake REQUIRED) -find_package(joint_state_controller REQUIRED) +find_package(joint_state_broadcaster REQUIRED) find_package(robot_state_publisher REQUIRED) find_package(rclcpp REQUIRED) find_package(gazebo_ros REQUIRED) @@ -29,8 +29,19 @@ find_package(rclpy REQUIRED) find_package(gazebo_msgs REQUIRED) find_package(std_srvs REQUIRED) -# Make sure to find Python 3.8 -find_package(Python3 3.8 EXACT COMPONENTS Interpreter Development REQUIRED) +# Make sure to find correct Python version based on ROS distro +if(DEFINED ENV{ROS_DISTRO}) + set(ROS_DISTRO_ENV $ENV{ROS_DISTRO}) + if(ROS_DISTRO_ENV STREQUAL "foxy") + find_package(Python3 3.8 EXACT COMPONENTS Interpreter Development REQUIRED) + elseif(ROS_DISTRO_ENV STREQUAL "humble") + find_package(Python3 3.10 EXACT COMPONENTS Interpreter Development REQUIRED) + else() + # Not very sure what would the default case be. A fuzzy match? + find_package(Python3 COMPONENTS Interpreter Development REQUIRED) + endif() +endif() + link_directories(/usr/local/lib) include_directories(${YAML_CPP_INCLUDE_DIR}) @@ -101,7 +112,7 @@ target_link_libraries(rl_real_go2 ) ament_target_dependencies(rl_sim - joint_state_controller + joint_state_broadcaster robot_state_publisher rclcpp gazebo_ros diff --git a/src/rl_sar/launch/gazebo.launch.py b/src/rl_sar/launch/gazebo.launch.py index e5cd5e7..e35fc6d 100644 --- a/src/rl_sar/launch/gazebo.launch.py +++ b/src/rl_sar/launch/gazebo.launch.py @@ -52,14 +52,14 @@ def generate_launch_description(): joint_state_broadcaster_node = Node( package="controller_manager", - executable="spawner.py", + executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner', arguments=["joint_state_broadcaster"], output="screen", ) robot_joint_controller_node = Node( package="controller_manager", - executable="spawner.py", + executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner', arguments=["robot_joint_controller"], output="screen", ) diff --git a/src/rl_sar/scripts/rl_sim.py b/src/rl_sar/scripts/rl_sim.py index b4ed030..7a85841 100755 --- a/src/rl_sar/scripts/rl_sim.py +++ b/src/rl_sar/scripts/rl_sim.py @@ -19,7 +19,7 @@ from ament_index_python.packages import get_package_share_directory from rl_sdk import * from observation_buffer import * -CSV_LOGGER = True +CSV_LOGGER = False class RL_Sim(RL, Node): def __init__(self): diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 44806d4..4030741 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -1,7 +1,7 @@ #include "rl_sim.hpp" // #define PLOT -#define CSV_LOGGER +// #define CSV_LOGGER RL_Sim::RL_Sim() : rclcpp::Node("rl_sim_node") @@ -12,31 +12,39 @@ RL_Sim::RL_Sim() param_client = this->create_client("/param_node/get_parameters"); while (!param_client->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + std::cout << LOGGER::ERROR << "Interrupted while waiting for param_node service. Exiting." << std::endl; + return; + } std::cout << LOGGER::WARNING << "Waiting for param_node service to be available..." << std::endl; } auto request = std::make_shared(); - request->names.push_back("robot_name"); - request->names.push_back("gazebo_model_name"); + request->names = {"robot_name", "gazebo_model_name"}; + + // Use a timeout for the future auto future = param_client->async_send_request(request); - rclcpp::spin_until_future_complete(this->get_node_base_interface(), future); - if (future.get()->values.size() < 2) + auto status = rclcpp::spin_until_future_complete(this->get_node_base_interface(), future, std::chrono::seconds(5)); + + if (status == rclcpp::FutureReturnCode::SUCCESS) { - std::cout << LOGGER::WARNING << "Failed to get all parameters from param_node" << std::endl; + auto result = future.get(); + if (result->values.size() < 2) + { + std::cout << LOGGER::ERROR << "Failed to get all parameters from param_node" << std::endl; + } + else + { + this->robot_name = result->values[0].string_value; + this->gazebo_model_name = result->values[1].string_value; + std::cout << LOGGER::INFO << "Get param robot_name: " << this->robot_name << std::endl; + std::cout << LOGGER::INFO << "Get param gazebo_model_name: " << this->gazebo_model_name << std::endl; + } } else { - this->robot_name = future.get()->values[0].string_value; - this->gazebo_model_name = future.get()->values[1].string_value; - std::cout << LOGGER::INFO << "Get param robot_name: " << this->robot_name << std::endl; - std::cout << LOGGER::INFO << "Get param gazebo_model_name: " << this->gazebo_model_name << std::endl; + std::cout << LOGGER::ERROR << "Failed to call param_node service" << std::endl; } - // // get params from this node - // this->declare_parameter("robot_name", ""); - // this->get_parameter("robot_name", this->robot_name); - // this->declare_parameter("gazebo_model_name", ""); - // this->get_parameter("gazebo_model_name", this->gazebo_model_name); - // read params from yaml this->ReadYaml(this->robot_name); for (std::string &observation : this->params.observations) diff --git a/src/robot_joint_controller/include/robot_joint_controller.hpp b/src/robot_joint_controller/include/robot_joint_controller.hpp index 7477a78..04770ad 100644 --- a/src/robot_joint_controller/include/robot_joint_controller.hpp +++ b/src/robot_joint_controller/include/robot_joint_controller.hpp @@ -37,7 +37,11 @@ typedef struct namespace robot_joint_controller { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +#if defined(ROS_DISTRO_FOXY) + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +#elif defined(ROS_DISTRO_HUMBLE) + using CallbackReturn = controller_interface::CallbackReturn; +#endif class RobotJointController : public controller_interface::ControllerInterface { @@ -51,13 +55,13 @@ public: #elif defined(ROS_DISTRO_HUMBLE) ROBOT_JOINT_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + ROBOT_JOINT_CONTROLLER_PUBLIC + CallbackReturn on_init() override; #endif ROBOT_JOINT_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; ROBOT_JOINT_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - // ROBOT_JOINT_CONTROLLER_PUBLIC - // CallbackReturn on_init() override; ROBOT_JOINT_CONTROLLER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; ROBOT_JOINT_CONTROLLER_PUBLIC diff --git a/src/robot_joint_controller/include/robot_joint_controller_group.hpp b/src/robot_joint_controller/include/robot_joint_controller_group.hpp index 0efb7cc..f3e1181 100644 --- a/src/robot_joint_controller/include/robot_joint_controller_group.hpp +++ b/src/robot_joint_controller/include/robot_joint_controller_group.hpp @@ -37,7 +37,11 @@ typedef struct namespace robot_joint_controller { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +#if defined(ROS_DISTRO_FOXY) + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +#elif defined(ROS_DISTRO_HUMBLE) + using CallbackReturn = controller_interface::CallbackReturn; +#endif class RobotJointControllerGroup : public controller_interface::ControllerInterface { @@ -51,13 +55,13 @@ public: #elif defined(ROS_DISTRO_HUMBLE) ROBOT_JOINT_CONTROLLER_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + ROBOT_JOINT_CONTROLLER_PUBLIC + CallbackReturn on_init() override; #endif ROBOT_JOINT_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; ROBOT_JOINT_CONTROLLER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - // ROBOT_JOINT_CONTROLLER_PUBLIC - // CallbackReturn on_init() override; ROBOT_JOINT_CONTROLLER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; ROBOT_JOINT_CONTROLLER_PUBLIC diff --git a/src/robot_joint_controller/src/robot_joint_controller.cpp b/src/robot_joint_controller/src/robot_joint_controller.cpp index 9d372e9..f1e6e39 100644 --- a/src/robot_joint_controller/src/robot_joint_controller.cpp +++ b/src/robot_joint_controller/src/robot_joint_controller.cpp @@ -18,10 +18,12 @@ RobotJointController::RobotJointController() memset(&servo_command_, 0, sizeof(ServoCommand)); } -// CallbackReturn RobotJointController::on_init() -// { -// return CallbackReturn::SUCCESS; -// } +#if defined(ROS_DISTRO_HUMBLE) +CallbackReturn RobotJointController::on_init() +{ + return CallbackReturn::SUCCESS; +} +#endif CallbackReturn RobotJointController::on_configure(const rclcpp_lifecycle::State &previous_state) { diff --git a/src/robot_joint_controller/src/robot_joint_controller_group.cpp b/src/robot_joint_controller/src/robot_joint_controller_group.cpp index 7e2bdf3..c0de6a2 100644 --- a/src/robot_joint_controller/src/robot_joint_controller_group.cpp +++ b/src/robot_joint_controller/src/robot_joint_controller_group.cpp @@ -16,10 +16,12 @@ RobotJointControllerGroup::RobotJointControllerGroup() memset(&servo_command_, 0, sizeof(ServoCommand)); } -// CallbackReturn RobotJointControllerGroup::on_init() -// { -// return CallbackReturn::SUCCESS; -// } +#if defined(ROS_DISTRO_HUMBLE) +CallbackReturn RobotJointControllerGroup::on_init() +{ + return CallbackReturn::SUCCESS; +} +#endif CallbackReturn RobotJointControllerGroup::on_configure(const rclcpp_lifecycle::State &previous_state) { diff --git a/src/robots/a1_description/launch/group_gazebo.launch.py b/src/robots/a1_description/launch/group_gazebo.launch.py index d77031b..26cf79a 100644 --- a/src/robots/a1_description/launch/group_gazebo.launch.py +++ b/src/robots/a1_description/launch/group_gazebo.launch.py @@ -38,14 +38,14 @@ def generate_launch_description(): joint_state_broadcaster_node = Node( package="controller_manager", - executable="spawner.py", + executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner', arguments=["joint_state_broadcaster"], output='screen', ) robot_joint_controller_node = Node( package="controller_manager", - executable="spawner.py", + executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner', arguments=["robot_joint_controller"], output='screen', ) diff --git a/src/robots/a1_description/launch/single_gazebo.launch.py b/src/robots/a1_description/launch/single_gazebo.launch.py index ff847f7..cbe8533 100644 --- a/src/robots/a1_description/launch/single_gazebo.launch.py +++ b/src/robots/a1_description/launch/single_gazebo.launch.py @@ -46,7 +46,7 @@ def generate_launch_description(): controller_nodes = [Node( package="controller_manager", - executable="spawner.py", + executable='spawner.py' if os.environ.get('ROS_DISTRO', '') == 'foxy' else 'spawner', arguments=[name], output='screen', ) for name in controller_names]