diff --git a/auto_driver_interface/CMakeLists.txt b/auto_driver_interface/CMakeLists.txt index 899a758..6fa8ab1 100644 --- a/auto_driver_interface/CMakeLists.txt +++ b/auto_driver_interface/CMakeLists.txt @@ -5,15 +5,35 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -# yaw_node -------------------------------------------------- +# pid_node -------------------------------------------------- set(TARGET pid_node) -ament_auto_add_library(${PROJECT_NAME} SHARED +ament_auto_add_library(${TARGET} SHARED src/${TARGET}.cpp) rclcpp_components_register_node( - ${PROJECT_NAME} + ${TARGET} PLUGIN "${PROJECT_NAME}::PidNode" - EXECUTABLE ${PROJECT_NAME}_exec) + EXECUTABLE ${TARGET}_exec) + +# auto_driver_interface::RollerNode +set(TARGET roller_node) +ament_auto_add_library(${TARGET} SHARED + src/${TARGET}.cpp) + +rclcpp_components_register_node( + ${TARGET} + PLUGIN "${PROJECT_NAME}::RollerNode" + EXECUTABLE ${TARGET}_exec) + +# auto_driver_interface::HammerNode +set(TARGET hammer_node) +ament_auto_add_library(${TARGET} SHARED + src/${TARGET}.cpp) + +rclcpp_components_register_node( + ${TARGET} + PLUGIN "${PROJECT_NAME}::HammerNode" + EXECUTABLE ${TARGET}_exec) ament_auto_package( INSTALL_TO_SHARE diff --git a/auto_driver_interface/launch/examle.launch.py b/auto_driver_interface/launch/examle.launch.py index 28c359a..bf1b60a 100644 --- a/auto_driver_interface/launch/examle.launch.py +++ b/auto_driver_interface/launch/examle.launch.py @@ -42,7 +42,25 @@ def generate_launch_description(): {'min_limit': 25}, {'max_limit': 315} ] - ) + ), + ComposableNode( + package='auto_driver_interface', + plugin='auto_driver_interface::RollerNode', + name='roller0', + namespace='can_node/c620_0', + parameters=[ + {'invert': False} + ] + ), + ComposableNode( + package='auto_driver_interface', + plugin='auto_driver_interface::RollerNode', + name='roller1', + namespace='can_node/c620_1', + parameters=[ + {'invert': True} + ] + ), ], ) ]) diff --git a/auto_driver_interface/src/roller_node.cpp b/auto_driver_interface/src/roller_node.cpp new file mode 100644 index 0000000..d60a346 --- /dev/null +++ b/auto_driver_interface/src/roller_node.cpp @@ -0,0 +1,65 @@ +// Copyright 2024 StrayedCats. +// +// 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 +#include + +namespace auto_driver_interface +{ + +class RollerNode : public rclcpp::Node +{ + +public: + RollerNode(rclcpp::NodeOptions options) + : Node("node", options) + { + this->declare_parameter("invert", false); + invert = this->get_parameter("invert").as_bool(); + + publisher_ = this->create_publisher("target_current", 1); + target_current_sub = this->create_subscription( + "/target_current", 1, std::bind(&RollerNode::target_current_callback, this, std::placeholders::_1)); + + timer_ = + this->create_wall_timer( + std::chrono::milliseconds(10), + std::bind(&RollerNode::timer_callback, this)); + } + +private: + void timer_callback() + { + auto message = std_msgs::msg::Float64(); + message.data = target_current * (invert ? -1 : 1); + publisher_->publish(message); + } + + void target_current_callback(const std_msgs::msg::Float64::SharedPtr msg) + { + target_current = msg->data; + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr target_current_sub; + rclcpp::TimerBase::SharedPtr timer_; + + float target_current = 0.0; + bool invert = false; +}; + +} // namespace auto_driver_interface + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(auto_driver_interface::RollerNode)