diff --git a/canopen/sphinx/user-guide/cia402-driver.rst b/canopen/sphinx/user-guide/cia402-driver.rst index 514e46999..7e7bb8be9 100644 --- a/canopen/sphinx/user-guide/cia402-driver.rst +++ b/canopen/sphinx/user-guide/cia402-driver.rst @@ -122,7 +122,7 @@ Additional parameters that can be used in bus.yml for this driver. - Milliseconds - Refresh period for 402 state machine. Should be similar to sync period of master. * - switching_state - - see below + - ? - The state to switch the operation mode in. * - scale_pos_to_dev - double diff --git a/canopen/sphinx/user-guide/configuration.rst b/canopen/sphinx/user-guide/configuration.rst index 76afdd762..10ac31af9 100644 --- a/canopen/sphinx/user-guide/configuration.rst +++ b/canopen/sphinx/user-guide/configuration.rst @@ -88,7 +88,7 @@ but come from the lely core library. Below you find a list of possible configura :widths: 1 1 configuration item; description - node_id; The node-ID (default: 255) + node_id; The node-ID (e.g. 255) driver; The fully qualified class name of the master to use. package; The ros2 package name in which the master class can be found. namespace; The namespace in which the master will be created (default: "/"). diff --git a/canopen/sphinx/user-guide/how-to-create-a-configuration.rst b/canopen/sphinx/user-guide/how-to-create-a-configuration.rst index 7bec65c7d..b33db2e61 100644 --- a/canopen/sphinx/user-guide/how-to-create-a-configuration.rst +++ b/canopen/sphinx/user-guide/how-to-create-a-configuration.rst @@ -104,7 +104,7 @@ Bus configuration creation .. code-block:: yaml options: - dcf_path: install/{package_name}/share/{package_name}/config/{bus_config_name} + dcf_path: "@BUS_CONFIG_PATH@" Then you need to define your master. @@ -126,7 +126,7 @@ Bus configuration creation .. code-block:: yaml nodes: - - [unique slave name]: + [unique slave name]: node_id: [node id] package: [ros2 package where to find the driver] driver: [qualified name of the driver] @@ -141,8 +141,7 @@ Create a launch folder in your package directory and a launch file. .. code-block:: console - mkdir launch - touch {...}.launch.py + touch launch/{...}.launch.py Add the following code: @@ -169,12 +168,6 @@ Add the following code: "{bus_config_name}", "master.dcf", ), - "master_bin": os.path.join( - get_package_share_directory("{package_name}"), - "config", - "{bus_config_name}", - "master.bin", - ), "bus_config": os.path.join( get_package_share_directory("{package_name}"), "config", @@ -206,36 +199,14 @@ Finally we need to adjust the CMakeLists.txt file to pick everything up correctl # find dependencies find_package(ament_cmake REQUIRED) - find_package(canopen_core REQUIRED) - find_package(canopen_interfaces REQUIRED) - find_package(canopen_base_driver REQUIRED) - find_package(canopen_proxy_driver REQUIRED) find_package(lely_core_libraries REQUIRED) cogen_dcf({bus_config_name}) install(DIRECTORY - launch/ - DESTINATION share/${PROJECT_NAME}/launch/ - ) - - install(DIRECTORY - launch_tests/ - DESTINATION share/${PROJECT_NAME}/launch_tests/ + launch + DESTINATION share/${PROJECT_NAME} ) - - if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - endif() - ament_package() diff --git a/canopen/sphinx/user-guide/how-to-create-a-robot-system.rst b/canopen/sphinx/user-guide/how-to-create-a-robot-system.rst index b9dcb489b..0865dfb8c 100644 --- a/canopen/sphinx/user-guide/how-to-create-a-robot-system.rst +++ b/canopen/sphinx/user-guide/how-to-create-a-robot-system.rst @@ -223,4 +223,4 @@ leveragin ros2_control. .. code-block:: bash - ros2 topic pub /joint1/forward_position_controller/command std_msgs/msg/Float64 "data: [1.0, 1.0]" + ros2 topic pub /joint_1/forward_position_controller/command std_msgs/msg/Float64 "data: [1.0, 1.0]" diff --git a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp index 4f3aa0a60..3293593a4 100644 --- a/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp +++ b/canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp @@ -45,7 +45,7 @@ void NodeCanopenBaseDriver::configure(bool call } RCLCPP_INFO_STREAM( this->node_->get_logger(), - "Non transmit timeout" << static_cast(this->non_transmit_timeout_.count()) << "ms"); + "Non transmit timeout " << static_cast(this->non_transmit_timeout_.count()) << "ms"); try { @@ -53,7 +53,7 @@ void NodeCanopenBaseDriver::configure(bool call } catch (...) { - RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true."); + RCLCPP_WARN(this->node_->get_logger(), "Could not read polling from config, setting to true."); polling_ = true; } if (polling_) @@ -78,7 +78,7 @@ void NodeCanopenBaseDriver::configure(bool call { RCLCPP_WARN( this->node_->get_logger(), - "Could not read enable diagnostics from config, setting to false."); + "Could not read `diagnostics/enable` from config, setting to false."); diagnostic_enabled_ = false; } if (diagnostic_enabled_.load()) @@ -91,7 +91,7 @@ void NodeCanopenBaseDriver::configure(bool call { RCLCPP_ERROR( this->node_->get_logger(), - "Could not read diagnostics period from config, setting to 1000ms"); + "Could not read `diagnostics/period` from config, setting to 1000ms"); diagnostic_period_ms_ = 1000; } @@ -132,7 +132,7 @@ void NodeCanopenBaseDriver::configure(bool called_from_base) } RCLCPP_INFO_STREAM( this->node_->get_logger(), - "Non transmit timeout" << static_cast(this->non_transmit_timeout_.count()) << "ms"); + "Non transmit timeout " << static_cast(this->non_transmit_timeout_.count()) << "ms"); try { @@ -140,7 +140,7 @@ void NodeCanopenBaseDriver::configure(bool called_from_base) } catch (...) { - RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true."); + RCLCPP_WARN(this->node_->get_logger(), "Could not read polling from config, setting to true."); polling_ = true; } if (polling_) @@ -165,7 +165,7 @@ void NodeCanopenBaseDriver::configure(bool called_from_base) { RCLCPP_WARN( this->node_->get_logger(), - "Could not read enable diagnostics from config, setting to false."); + "Could not read `diagnostics/enable` from config, setting to false."); diagnostic_enabled_ = false; } if (diagnostic_enabled_.load()) @@ -178,7 +178,7 @@ void NodeCanopenBaseDriver::configure(bool called_from_base) { RCLCPP_WARN( this->node_->get_logger(), - "Could not read diagnostics period from config, setting to 1000ms"); + "Could not read `diagnostics/period` from config, setting to 1000ms"); diagnostic_period_ms_ = 1000; } @@ -294,7 +294,7 @@ void NodeCanopenBaseDriver::add_to_master() bool boot_success = false; int boot_attempts = 0; const int max_boot_attempts = 3; // 1 retry allowed - RCLCPP_WARN(this->node_->get_logger(), "Wait for device to boot..."); + RCLCPP_INFO(this->node_->get_logger(), "Wait for device to boot..."); while (!boot_success && boot_attempts < max_boot_attempts) { boot_attempts++; diff --git a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp index 29ec2375e..58dd8d0a0 100644 --- a/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp +++ b/canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp @@ -40,7 +40,7 @@ namespace node_interfaces * @brief Node Canopen Master * * This class implements the NodeCanopenMasterInterface. It provides - * core functionality and logic for CanopenMaster, indepentently of the + * core functionality and logic for CanopenMaster, independently of the * ROS node type. Currently rclcpp::Node and rclcpp_lifecycle::LifecycleNode * and derived classes are supported. Other node types will lead to compile * time error. @@ -85,7 +85,7 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface std::string master_dcf_; std::string master_bin_; std::string can_interface_name_; - uint32_t timeout_; + uint32_t timeout_ = 2000; std::thread spinner_; @@ -169,18 +169,17 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface virtual void configure(bool called_from_base) { - std::optional timeout; try { - timeout = this->config_["boot_timeout"].as(); + this->timeout_ = this->config_["boot_timeout"].as(); } catch (...) { RCLCPP_WARN( this->node_->get_logger(), - "No timeout parameter found in config file. Using default value of 100ms."); + "No `boot_timeout` parameter found in config file. Using default value of %dms.", + this->timeout_); } - this->timeout_ = timeout.value_or(2000); RCLCPP_INFO_STREAM( this->node_->get_logger(), "Master boot timeout set to " << this->timeout_ << "ms."); } diff --git a/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp b/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp index 20eb070e0..55333954d 100644 --- a/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp +++ b/canopen_ros2_control/include/canopen_ros2_control/cia402_data.hpp @@ -58,12 +58,12 @@ struct Cia402Data if (!config["node_id"]) { - RCLCPP_ERROR(rclcpp::get_logger(joint_name), "No node id for '%s'", joint.name.c_str()); + RCLCPP_INFO(rclcpp::get_logger(joint_name), "No node id for '%s'", joint.name.c_str()); return false; } node_id = config["node_id"].as(); - RCLCPP_ERROR( + RCLCPP_INFO( rclcpp::get_logger(joint_name), "Node id for '%s' is '%u'", joint.name.c_str(), node_id); if (config["position_mode"])