diff --git a/aichallenge/workspace/src/aichallenge_submit/param_controller/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/param_controller/CMakeLists.txt new file mode 100644 index 00000000..8c87775b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/param_controller/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.8) +project(param_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_executable(param_controller_node + src/param_controller.cpp +) + +ament_auto_package(INSTALL_TO_SHARE +) diff --git a/aichallenge/workspace/src/aichallenge_submit/param_controller/package.xml b/aichallenge/workspace/src/aichallenge_submit/param_controller/package.xml new file mode 100644 index 00000000..c4a00f20 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/param_controller/package.xml @@ -0,0 +1,20 @@ + + + + param_controller + 0.0.0 + TODO: Package description + Yu Asabe + TODO: License declaration + + ament_cmake_auto + + rclcpp + tier4_vehicle_msgs + geometry_msgs + sensor_msgs + + + ament_cmake + + diff --git a/aichallenge/workspace/src/aichallenge_submit/param_controller/src/param_controller.cpp b/aichallenge/workspace/src/aichallenge_submit/param_controller/src/param_controller.cpp new file mode 100644 index 00000000..85b1ccfd --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/param_controller/src/param_controller.cpp @@ -0,0 +1,69 @@ +#include +#include + +namespace param_controller +{ + +class ParamController : public rclcpp::Node +{ +public: + ParamController() + : Node("param_controller"), steering_value_(0.0) + { + publisher_ = this->create_publisher( + "/control/command/actuation_cmd", 10); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(50), + std::bind(&ParamController::timer_callback, this)); + + // Parameter declaration + this->declare_parameter("steering_angle", 0.0); + + // Parameter callback + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&ParamController::param_callback, this, std::placeholders::_1)); + } + +private: + void timer_callback() + { + auto message = tier4_vehicle_msgs::msg::ActuationCommandStamped(); + message.header.stamp = this->now(); + message.actuation.steer_cmd = steering_value_; + publisher_->publish(message); + RCLCPP_INFO(this->get_logger(), "Publishing: steering_angle = %.2f", steering_value_); + } + + rcl_interfaces::msg::SetParametersResult param_callback( + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + if (param.get_name() == "steering_angle") { + steering_value_ = param.as_double(); + RCLCPP_INFO(this->get_logger(), "Parameter updated: steering_angle = %.2f", steering_value_); + } + } + + return result; + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + double steering_value_; +}; + +} // namespace param_controller + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file