#!/usr/bin/env python3
"""
Convert a ROS 2 bag file to CSV format.

This script reads messages from a ROS 2 bag file and converts them to CSV format,
creating separate CSV files for each topic in the bag.

Usage:
  python3 ros2bag_to_csv.py <input_bag_path> <output_folder>

Subscription Topics:
    None (reads from bag file)

Publishing Topics:
    None (writes to CSV files)

:author: Addison Sears-Collins
:date: November 26, 2024
"""

import argparse
import csv
import os

from rclpy.serialization import deserialize_message
from rosidl_runtime_py.utilities import get_message
import rosbag2_py
from std_msgs.msg import String


def read_messages(input_bag: str):
    """
    Read messages from a ROS 2 bag file.

    :param input_bag: Path to the input bag file.
    :return: A generator yielding (topic, message, timestamp) tuples.
    """
    # Create a SequentialReader to read messages from the bag file
    reader = rosbag2_py.SequentialReader()

    # Open the bag file for reading
    reader.open(
        rosbag2_py.StorageOptions(uri=input_bag, storage_id="mcap"),
        rosbag2_py.ConverterOptions(
            input_serialization_format="cdr", output_serialization_format="cdr"
        ),
    )

    # Get all the topic types available in the bag file
    topic_types = reader.get_all_topics_and_types()

    def typename(topic_name):
        """
        Get the message type for a given topic.

        :param topic_name: The name of the topic.
        :return: The message type as a string.
        :raises ValueError: If the topic is not found in the bag.
        """
        # Iterate through the topic types to find the matching topic name
        for topic_type in topic_types:
            if topic_type.name == topic_name:
                return topic_type.type
        raise ValueError(f"topic {topic_name} not in bag")

    # Iterate through the messages in the bag file
    while reader.has_next():
        topic, data, timestamp = reader.read_next()
        msg_type = get_message(typename(topic))
        msg = deserialize_message(data, msg_type)
        yield topic, msg, timestamp

    # Clean up the reader
    del reader


def main():
    """
    Main function to parse arguments and convert ROS 2 bag to CSV.
    """
    # Create an argument parser
    parser = argparse.ArgumentParser(description="Convert ROS 2 bag to CSV")
    parser.add_argument("input", help="Input bag path (folder) to read from")
    parser.add_argument("output", help="Output folder to save CSV files")

    # Parse the command-line arguments
    args = parser.parse_args()

    # Create the output folder if it doesn't exist
    if not os.path.exists(args.output):
        os.makedirs(args.output)

    # Initialize dictionaries to store topic files and writers
    topic_files = {}
    topic_writers = {}

    # Iterate through the messages in the bag file
    for topic, msg, timestamp in read_messages(args.input):
        # If the topic hasn't been processed before, create a new CSV file and writer
        if topic not in topic_files:
            output_file = os.path.join(args.output, topic.replace('/', '_') + '.csv')
            topic_files[topic] = open(output_file, mode='w',
                                      newline='', encoding='utf-8')
            topic_writers[topic] = csv.writer(topic_files[topic])
            topic_writers[topic].writerow(['timestamp', 'data'])

        # Write the message data to the corresponding CSV file
        if isinstance(msg, String):
            topic_writers[topic].writerow([timestamp, msg.data])
        else:
            topic_writers[topic].writerow([timestamp, str(msg)])

    # Close all the topic files
    for file in topic_files.values():
        file.close()


if __name__ == "__main__":
    main()

# Request parameters for the timed rotation
float64 angular_velocity    # Desired angular velocity in rad/s (positive for counterclockwise, negative for clockwise)
float64 duration           # Desired duration of the rotation in seconds
---
# Result of the completed rotation
bool success               # Indicates if the rotation was completed successfully
float64 actual_duration    # Actual duration of the rotation in seconds
---
# Continuous feedback during rotation
float64 elapsed_time  # Elapsed time since the start of the rotation in seconds
string status         # Current status of the rotation:
                      # GOAL_RECEIVED: The action server has received a goal
                      # ROTATING: The rotation is in progress
                      # GOAL_SUCCEEDED: The rotation has been completed successfully
                      # GOAL_ABORTED: The rotation has been aborted due to an error or exceptional condition
                      # GOAL_CANCELED: The client has canceled the rotation goal

cmake_minimum_required(VERSION 3.8)
project(yahboom_rosmaster_msgs)

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 REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "action/TimedRotation.action"
)

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()

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>yahboom_rosmaster_msgs</name>
  <version>0.0.0</version>
  <description>Custom messages, services, and actions for the Yahboom ROSMaster robot</description>
  <maintainer email="automaticaddison@todo.com">ubuntu</maintainer>
  <license>BSD-3-Clause</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>rosidl_default_generators</buildtool_depend>

  <exec_depend>rosidl_default_runtime</exec_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

/**
 * @file timed_rotation_action_server.cpp
 * @brief This ROS 2 node implements an action server for the TimedRotation action.
 *
 * This program implements a ROS 2 action server that executes timed rotation actions for the Yahboom RosMaster robot.
 * It receives goals from action clients, publishes timestamped velocity commands to rotate the robot,
 * and provides feedback on the rotation progress.
 *
 * Subscription Topics:
 *   None
 *
 * Publishing Topics:
 *   /mecanum_drive_controller/cmd_vel (geometry_msgs/TwistStamped):
 *     Timestamped velocity command for the robot:
 *     - header.stamp: Current ROS time when command is published
 *     - header.frame_id: "base_link" (robot's base frame)
 *     - twist.linear.x: Always 0 (no forward/backward motion)
 *     - twist.linear.y: Always 0 (no left/right motion)
 *     - twist.angular.z: Rotation speed in radians/second (positive=counterclockwise, negative=clockwise)
 *
 * Action Server:
 *   timed_rotation (yahboom_rosmaster_msgs/action/TimedRotation):
 *     Executes timed rotation actions with the following:
 *     - Goal: angular_velocity (rad/s) and duration (seconds)
 *     - Feedback: elapsed_time and status updates
 *     - Result: success flag and actual_duration
 *
 * @author Addison Sears-Collins
 * @date November 25, 2024
 */

#include <memory>
#include <thread>
#include <chrono>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "yahboom_rosmaster_msgs/action/timed_rotation.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"

using namespace std::chrono_literals;
using TimedRotation = yahboom_rosmaster_msgs::action::TimedRotation;
using GoalHandleTimedRotation = rclcpp_action::ServerGoalHandle<TimedRotation>;

/**
 * @class TimedRotationActionServer
 * @brief ROS 2 action server for the TimedRotation action.
 *
 * This class implements the action server functionality for the TimedRotation action.
 * It receives goals from an action client, executes the timed rotation, provides feedback,
 * and handles goal cancellation requests. The server maintains rotation by continuously
 * publishing timestamped velocity commands until the specified duration is reached or the goal is canceled.
 */
class TimedRotationActionServer : public rclcpp::Node
{
public:
  /**
   * @brief Constructor for the TimedRotationActionServer class.
   *
   * Initializes the action server and the timestamped velocity command publisher.
   * Sets up callback bindings for handling goals, cancellation requests,
   * and accepted goals.
   */
  TimedRotationActionServer()
  : Node("timed_rotation_action_server_cpp")
  {
    using namespace std::placeholders;

    // Create the action server with callbacks for goal handling, cancellation, and execution
    action_server_ = rclcpp_action::create_server<TimedRotation>(
      this,
      "timed_rotation",
      std::bind(&TimedRotationActionServer::handle_goal, this, _1, _2),
      std::bind(&TimedRotationActionServer::handle_cancel, this, _1),
      std::bind(&TimedRotationActionServer::handle_accepted, this, _1));

    // Create publisher for timestamped velocity commands
    cmd_vel_publisher_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(
      "/mecanum_drive_controller/cmd_vel",
      10);  // Queue size of 10 messages
  }

private:
  // Class member variables
  rclcpp_action::Server<TimedRotation>::SharedPtr action_server_;  ///< The action server instance
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_publisher_;  ///< Publisher for timestamped velocity commands

  /**
   * @brief Callback function for handling new goal requests.
   *
   * This function is called when a new goal is received from an action client.
   * Currently accepts all goals without any validation.
   *
   * @param uuid Unique identifier for the goal request.
   * @param goal The goal request message containing angular_velocity and duration.
   * @return rclcpp_action::GoalResponse indicating if the goal is accepted or rejected.
   */
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    [[maybe_unused]] std::shared_ptr<const TimedRotation::Goal> goal)
  {
    RCLCPP_INFO(this->get_logger(), "Received goal request for a timed rotation action");
    (void)uuid;
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  /**
   * @brief Callback function for handling cancel requests.
   *
   * This function is called when a client requests to cancel an ongoing goal.
   * Currently accepts all cancellation requests.
   *
   * @param goal_handle The goal handle associated with the goal to be canceled.
   * @return rclcpp_action::CancelResponse indicating if the cancellation is accepted.
   */
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleTimedRotation> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Received cancel request for timed rotation action");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  /**
   * @brief Callback function for handling accepted goals.
   *
   * This function is called when a goal has been accepted and needs to be executed.
   * Spawns a new thread to execute the goal to avoid blocking the main thread.
   *
   * @param goal_handle The goal handle associated with the accepted goal.
   */
  void handle_accepted(const std::shared_ptr<GoalHandleTimedRotation> goal_handle)
  {
    // Create a new thread using proper bind syntax
    std::thread{
      [this, goal_handle]() {
        this->execute(goal_handle);
      }
    }.detach();
  }

  /**
   * @brief Executes the timed rotation action.
   *
   * This function runs in a separate thread and performs the following:
   * 1. Publishes initial feedback with GOAL_RECEIVED status
   * 2. Creates and sends timestamped velocity commands based on goal parameters
   * 3. Continuously publishes feedback during rotation
   * 4. Handles cancellation requests during execution
   * 5. Stops rotation and publishes final result
   *
   * @param goal_handle The goal handle associated with the goal to be executed.
   */
  void execute(const std::shared_ptr<GoalHandleTimedRotation> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "Executing goal for timed rotation action...");

    // Get the goal message
    const auto goal = goal_handle->get_goal();

    // Create result and feedback messages
    auto result = std::make_shared<TimedRotation::Result>();
    auto feedback = std::make_shared<TimedRotation::Feedback>();

    // Send initial feedback
    feedback->status = "GOAL_RECEIVED";
    goal_handle->publish_feedback(feedback);

    // Create timestamped velocity command message
    geometry_msgs::msg::TwistStamped twist_stamped_msg;
    twist_stamped_msg.twist.angular.z = goal->angular_velocity;  // Set rotation speed
    twist_stamped_msg.header.frame_id = "base_link";  // Set the reference frame
    // linear.x and linear.y are automatically initialized to 0

    // Record start time for duration tracking
    auto start_time = this->now();

    // Main execution loop
    while (rclcpp::ok())
    {
      // Check for cancellation requests
      if (goal_handle->is_canceling())
      {
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal canceled");
        feedback->status = "GOAL_CANCELED";
        goal_handle->publish_feedback(feedback);

        // Stop the robot with timestamped command
        twist_stamped_msg.twist.angular.z = 0.0;
        twist_stamped_msg.header.stamp = this->now();
        cmd_vel_publisher_->publish(twist_stamped_msg);
        return;
      }

      // Update timestamp and publish velocity command
      twist_stamped_msg.header.stamp = this->now();
      cmd_vel_publisher_->publish(twist_stamped_msg);

      // Calculate elapsed time
      auto elapsed_time = (this->now() - start_time).seconds();

      // Publish feedback
      feedback->elapsed_time = elapsed_time;
      feedback->status = "ROTATING";
      goal_handle->publish_feedback(feedback);

      // Check if we've reached the desired duration
      if (elapsed_time >= goal->duration)
      {
        break;
      }

      // Sleep to control update rate (5Hz)
      std::this_thread::sleep_for(200ms);
    }

    // Stop the robot with final timestamped command
    twist_stamped_msg.twist.angular.z = 0.0;
    twist_stamped_msg.header.stamp = this->now();
    cmd_vel_publisher_->publish(twist_stamped_msg);

    // Set and send result
    result->success = true;
    result->actual_duration = (this->now() - start_time).seconds();

    // Mark the goal as succeeded
    goal_handle->succeed(result);

    // Send final feedback
    feedback->status = "GOAL_SUCCEEDED";
    goal_handle->publish_feedback(feedback);

    RCLCPP_INFO(this->get_logger(), "Goal succeeded");
  }
};

/**
 * @brief Main function for the timed rotation action server node.
 *
 * Initializes ROS 2, creates an instance of the TimedRotationActionServer,
 * and spins the node to process callbacks.
 *
 * @param argc Number of command-line arguments.
 * @param argv Array of command-line arguments.
 * @return int Exit status of the program.
 */
int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto action_server = std::make_shared<TimedRotationActionServer>();
  rclcpp::spin(action_server);
  rclcpp::shutdown();
  return 0;
}

/**
 * @file timed_rotation_action_client.cpp
 * @brief ROS 2 action client node for sending timed rotation goals.
 *
 * This program implements a ROS 2 action client node that sends timed rotation goals
 * to a corresponding action server. It subscribes to the /timed_rotation_mode topic
 * to determine when to send or cancel goals.
 *
 * Subscription Topics:
 *   /timed_rotation_mode (std_msgs/Bool): Topic indicating whether timed rotation mode is active
 *
 * Publishing Topics:
 *   None
 *
 * Action Client:
 *   timed_rotation (yahboom_rosmaster_msgs/action/TimedRotation):
 *     Sends goals for timed rotation and receives feedback and results:
 *     - Goal: angular_velocity (rad/s) and duration (seconds)
 *     - Feedback: elapsed_time (seconds) and status string
 *     - Result: success flag and actual_duration (seconds)
 *
 * @author Addison Sears-Collins
 * @date November 25, 2024
 */

#include <memory>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "yahboom_rosmaster_msgs/action/timed_rotation.hpp"
#include "std_msgs/msg/bool.hpp"

using TimedRotation = yahboom_rosmaster_msgs::action::TimedRotation;
using GoalHandleTimedRotation = rclcpp_action::ClientGoalHandle<TimedRotation>;

/**
 * @class TimedRotationActionClient
 * @brief ROS 2 action client node for sending timed rotation goals.
 *
 * This class implements a ROS 2 action client that sends timed rotation goals and
 * processes feedback and results from the action server. It can be controlled via
 * a boolean topic to start and stop rotations.
 */
class TimedRotationActionClient : public rclcpp::Node
{
public:
  using TimedRotation = yahboom_rosmaster_msgs::action::TimedRotation;
  using GoalHandleTimedRotation = rclcpp_action::ClientGoalHandle<TimedRotation>;

  /**
   * @brief Constructor for the TimedRotationActionClient class.
   * @param options ROS 2 node options.
   */
  explicit TimedRotationActionClient(const rclcpp::NodeOptions& options)
  : Node("timed_rotation_action_client_cpp", options)
  {
    // Create the action client
    action_client_ = rclcpp_action::create_client<TimedRotation>(
      this,
      "timed_rotation"
    );

    // Create subscription to control rotation mode
    timed_rotation_mode_sub_ = create_subscription<std_msgs::msg::Bool>(
      "/timed_rotation_mode",
      10,
      std::bind(&TimedRotationActionClient::timed_rotation_mode_callback, this, std::placeholders::_1)
    );

    // Declare parameters with default values
    this->declare_parameter("angular_velocity", 0.5);  // rad/s
    this->declare_parameter("duration", 10.0);         // seconds
  }

private:
  /**
   * @brief Callback function for the /timed_rotation_mode topic.
   *
   * Controls the action client based on the received boolean value:
   * - True: Starts a new rotation if none is active
   * - False: Cancels the current rotation if one is active
   *
   * @param msg The received boolean message.
   */
  void timed_rotation_mode_callback(const std_msgs::msg::Bool::SharedPtr msg)
  {
    if (!timed_rotation_mode_ && msg->data)
    {
      // Transition from False to True - Start rotation
      send_goal();
    }
    else if (timed_rotation_mode_ && !msg->data)
    {
      // Transition from True to False - Cancel if active
      if (goal_handle_ && goal_handle_->get_status() == rclcpp_action::GoalStatus::STATUS_ACCEPTED)
      {
        cancel_goal();
      }
    }
    timed_rotation_mode_ = msg->data;
  }

  /**
   * @brief Callback function for goal response.
   * @param goal_handle The goal handle returned by the action server.
   */
  void goal_response_callback(const GoalHandleTimedRotation::SharedPtr& goal_handle)
  {
    if (!goal_handle)
    {
      RCLCPP_ERROR(get_logger(), "Goal was rejected by server");
      return;
    }

    goal_handle_ = goal_handle;
    RCLCPP_INFO(get_logger(), "Goal accepted by server");
  }

  /**
   * @brief Callback function for feedback.
   * @param goal_handle The goal handle associated with the feedback.
   * @param feedback The feedback message received from the action server.
   */
  void feedback_callback(
    const GoalHandleTimedRotation::SharedPtr& goal_handle,
    const std::shared_ptr<const TimedRotation::Feedback>& feedback)
  {
    (void)goal_handle;  // Unused parameter
    RCLCPP_INFO(get_logger(), "Feedback received - Elapsed time: %.2f seconds, Status: %s",
                feedback->elapsed_time,
                feedback->status.c_str());
  }

  /**
   * @brief Callback function for getting the result.
   * @param result The result message received from the action server.
   */
  void get_result_callback(const GoalHandleTimedRotation::WrappedResult& result)
  {
    switch (result.code)
    {
      case rclcpp_action::ResultCode::SUCCEEDED:
        RCLCPP_INFO(get_logger(), "Goal succeeded! Actual duration: %.2f seconds",
                    result.result->actual_duration);
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_INFO(get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(get_logger(), "Unknown result code");
        return;
    }
  }

  /**
   * @brief Cancel the current goal.
   *
   * Attempts to cancel the currently executing goal. Checks for a valid goal handle
   * and waits for the cancellation response from the server.
   */
  void cancel_goal()
  {
    RCLCPP_INFO(get_logger(), "Requesting to cancel goal");
    if (!goal_handle_)
    {
        RCLCPP_ERROR(get_logger(), "Goal handle is null");
        return;
    }

    auto future = action_client_->async_cancel_goal(goal_handle_);

    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) !=
        rclcpp::FutureReturnCode::SUCCESS)
    {
        RCLCPP_ERROR(get_logger(), "Failed to cancel goal");
        return;
    }
    RCLCPP_INFO(get_logger(), "Goal cancellation request sent");
  }

  /**
   * @brief Send a goal to the action server.
   *
   * Sends a new rotation goal using parameters from the parameter server.
   * If the server isn't available, it will wait for it to become available.
   */
  void send_goal()
  {
    RCLCPP_INFO(get_logger(), "Waiting for action server...");
    if (!action_client_->wait_for_action_server(std::chrono::seconds(5)))
    {
      RCLCPP_ERROR(get_logger(), "Action server not available after 5 seconds");
      return;
    }

    // Get parameters for the goal
    auto angular_velocity = this->get_parameter("angular_velocity").as_double();
    auto duration = this->get_parameter("duration").as_double();

    auto goal_msg = TimedRotation::Goal();
    goal_msg.angular_velocity = angular_velocity;
    goal_msg.duration = duration;

    RCLCPP_INFO(get_logger(),
                "Sending goal: angular_velocity=%.2f rad/s, duration=%.2f seconds",
                angular_velocity, duration);

    auto send_goal_options = rclcpp_action::Client<TimedRotation>::SendGoalOptions();
    send_goal_options.goal_response_callback =
      std::bind(&TimedRotationActionClient::goal_response_callback, this, std::placeholders::_1);
    send_goal_options.feedback_callback =
      std::bind(&TimedRotationActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
    send_goal_options.result_callback =
      std::bind(&TimedRotationActionClient::get_result_callback, this, std::placeholders::_1);

    action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // Class member variables
  rclcpp_action::Client<TimedRotation>::SharedPtr action_client_;        ///< Action client for timed rotation
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr timed_rotation_mode_sub_;  ///< Subscription to timed rotation mode
  bool timed_rotation_mode_{false};                                      ///< Current state of timed rotation mode
  GoalHandleTimedRotation::SharedPtr goal_handle_;                      ///< Handle for the current goal
};

/**
 * @brief Main function for the timed rotation action client node.
 *
 * Initializes ROS 2, creates an instance of the TimedRotationActionClient,
 * and spins the node to process callbacks.
 *
 * @param argc Number of command-line arguments.
 * @param argv Array of command-line arguments.
 * @return int Exit status of the program.
 */
int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto action_client = std::make_shared<TimedRotationActionClient>(rclcpp::NodeOptions());
  rclcpp::spin(action_client);
  rclcpp::shutdown();
  return 0;
}

/**
 * @file mecanum_parameters_node.cpp
 * @brief This ROS 2 node demonstrates how to declare parameters and publish velocity messages.
 *
 * This program implements a ROS 2 node that declares parameters for a mecanum wheel robot 
 * and publishes velocity commands. It demonstrates parameter declaration, callback
 * registration for parameter changes, and publishing to the velocity topic.
 *
 * Subscription Topics:
 *   None
 *
 * Publishing Topics:
 *   /mecanum_drive_controller/cmd_vel (geometry_msgs/TwistStamped): Velocity command for the robot
 *   (linear.x, linear.y, and angular.z)
 *
 * Parameters:
 *   robot_name (string): The name of the robot
 *   robot_id (int): The unique ID number of the robot
 *   target_linear_velocity_x (double): Target linear velocity in x direction (m/s)
 *   target_linear_velocity_y (double): Target linear velocity in y direction (m/s)
 *   target_angular_velocity (double): Target angular velocity in radians per second
 *
 * @author Addison Sears-Collins
 * @date November 24, 2024
 */

#include <rclcpp/rclcpp.hpp>
#include <rcl_interfaces/msg/parameter_descriptor.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>

/**
 * @class MecanumParameters
 * @brief A ROS 2 node that demonstrates parameter handling and velocity publishing.
 */
class MecanumParameters : public rclcpp::Node
{
public:
  /**
   * @brief Constructor for the MecanumParameters node.
   *
   * Initializes the node, declares parameters, sets up parameter change callback,
   * and creates a publisher and timer for velocity messages.
   */
  MecanumParameters() : Node("mecanum_parameters_node")
  {
    // Describe parameters
    rcl_interfaces::msg::ParameterDescriptor robot_name_descriptor;
    robot_name_descriptor.description = "The name of the robot";
    rcl_interfaces::msg::ParameterDescriptor robot_id_descriptor;
    robot_id_descriptor.description = "The unique ID number of the robot";
    rcl_interfaces::msg::ParameterDescriptor target_linear_velocity_x_descriptor;
    target_linear_velocity_x_descriptor.description = "Target linear velocity in x direction (meters per second)";
    rcl_interfaces::msg::ParameterDescriptor target_linear_velocity_y_descriptor;
    target_linear_velocity_y_descriptor.description = "Target linear velocity in y direction (meters per second)";
    rcl_interfaces::msg::ParameterDescriptor target_angular_velocity_descriptor;
    target_angular_velocity_descriptor.description = "Target angular velocity in radians per second";

    // Declare parameters using declare_parameter()
    this->declare_parameter("robot_name", "Automatic Addison Bot", robot_name_descriptor);
    this->declare_parameter("robot_id", 1, robot_id_descriptor);
    this->declare_parameter("target_linear_velocity_x", 0.0, target_linear_velocity_x_descriptor);
    this->declare_parameter("target_linear_velocity_y", 0.0, target_linear_velocity_y_descriptor);
    this->declare_parameter("target_angular_velocity", 0.0, target_angular_velocity_descriptor);

    // Register a callback function for parameter changes using the newer method
    param_callback_handle_ = this->add_on_set_parameters_callback(
      std::bind(&MecanumParameters::parameter_change_callback, this, std::placeholders::_1));

    // Create a publisher for the /mecanum_drive_controller/cmd_vel topic
    velocity_publisher_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("/mecanum_drive_controller/cmd_vel", 10);

    // Create a timer that will call the publish_velocity function every 0.5 seconds (2 Hz)
    timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&MecanumParameters::publish_velocity, this));
  }

private:
  /**
   * @brief Callback function for parameter changes.
   *
   * @param params A vector of rclcpp::Parameter objects representing the parameters that are
   *               being attempted to change.
   * @return rcl_interfaces::msg::SetParametersResult Object indicating whether the change was successful.
   */
  rcl_interfaces::msg::SetParametersResult parameter_change_callback(const std::vector<rclcpp::Parameter> & params)
  {
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = true;

    for (const auto & param : params)
    {
      if (param.get_name() == "robot_name" && param.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
      {
        RCLCPP_INFO(this->get_logger(), "Parameter robot_name has changed. The new value is: %s", param.as_string().c_str());
      }
      else if (param.get_name() == "robot_id" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
      {
        RCLCPP_INFO(this->get_logger(), "Parameter robot_id has changed. The new value is: %ld", param.as_int());  // Changed %d to %ld
      }
      else if (param.get_name() == "target_linear_velocity_x" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
      {
        RCLCPP_INFO(this->get_logger(), "Parameter target_linear_velocity_x has changed. The new value is: %f", param.as_double());
      }
      else if (param.get_name() == "target_linear_velocity_y" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
      {
        RCLCPP_INFO(this->get_logger(), "Parameter target_linear_velocity_y has changed. The new value is: %f", param.as_double());
      }
      else if (param.get_name() == "target_angular_velocity" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
      {
        RCLCPP_INFO(this->get_logger(), "Parameter target_angular_velocity has changed. The new value is: %f", param.as_double());
      }
      else
      {
        result.successful = false;
        result.reason = "Unknown parameter: " + param.get_name();
        break;
      }
    }
    return result;
  }

  /**
   * @brief Publishes velocity messages to the /mecanum_drive_controller/cmd_vel topic.
   *
   * This function is called periodically by the timer to publish the current
   * target linear and angular velocities.
   */
  void publish_velocity()
  {
    double target_linear_velocity_x = this->get_parameter("target_linear_velocity_x").as_double();
    double target_linear_velocity_y = this->get_parameter("target_linear_velocity_y").as_double();
    double target_angular_velocity = this->get_parameter("target_angular_velocity").as_double();

    geometry_msgs::msg::TwistStamped velocity_msg;
    velocity_msg.header.stamp = this->get_clock()->now();
    velocity_msg.header.frame_id = "base_link";
    velocity_msg.twist.linear.x = target_linear_velocity_x;
    velocity_msg.twist.linear.y = target_linear_velocity_y;
    velocity_msg.twist.angular.z = target_angular_velocity;

    velocity_publisher_->publish(velocity_msg);
  }

  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_publisher_;  ///< Publisher for velocity commands
  rclcpp::TimerBase::SharedPtr timer_;  ///< Timer for periodic velocity publishing
  OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;  ///< Handle for the parameter callback
};

/**
 * @brief Main function to start the ROS 2 node.
 *
 * Initializes ROS 2, creates an instance of the MecanumParameters node,
 * and keeps it running to process callbacks and publish messages.
 *
 * @param argc Number of command-line arguments.
 * @param argv Array of command-line argument strings.
 * @return int Return code (0 for normal exit).
 */
int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto mecanum_parameters_node = std::make_shared<MecanumParameters>();
  rclcpp::spin(mecanum_parameters_node);
  rclcpp::shutdown();
  return 0;
}

<depend>rcl_interfaces</depend>

cmake_minimum_required(VERSION 3.8)
project(yahboom_rosmaster_system_tests)

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 REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rcl_interfaces REQUIRED)

# Copy necessary files to designated locations in the project
install (
  DIRECTORY config launch
  DESTINATION share/${PROJECT_NAME}
)

# Add the square mecanum controller executable
add_executable(square_mecanum_controller
  src/square_mecanum_controller.cpp
)

# Add the mecanum parameters node executable
add_executable(mecanum_parameters_node
  src/mecanum_parameters_node.cpp
)

# Specify dependencies for the target
ament_target_dependencies(square_mecanum_controller
  rclcpp
  geometry_msgs
  std_msgs
)

# Specify dependencies for the mecanum parameters node
ament_target_dependencies(mecanum_parameters_node
  rclcpp
  geometry_msgs
  std_msgs
  rcl_interfaces
)

# Install the executables
install(TARGETS
  square_mecanum_controller
  mecanum_parameters_node
  DESTINATION lib/${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()

#!/usr/bin/env python3
"""
Launch file for mecanum_parameters_node.

This launch file launches the mecanum_parameters_node with configurable parameters.

:author: Addison Sears-Collins
:date: November 24, 2024
"""

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    """
    Generate a launch description for the mecanum_parameters_node.

    :return: A LaunchDescription object containing the actions to execute.
    :rtype: LaunchDescription
    """

    # Constants for paths to different files and folders
    package_name_system_tests = 'yahboom_rosmaster_system_tests'

    mecanum_params_path = 'config/mecanum_parameters.yaml'

    # Set the path to different files and folders
    pkg_share_system_tests = FindPackageShare(
        package=package_name_system_tests).find(package_name_system_tests)

    mecanum_params_path = os.path.join(pkg_share_system_tests, mecanum_params_path)

    # Launch configuration variables
    mecanum_params = LaunchConfiguration('mecanum_params')

    # Declare the launch arguments
    declare_mecanum_params_cmd = DeclareLaunchArgument(
        name='mecanum_params',
        default_value=mecanum_params_path,
        description='Full path to parameters for the parameters file.')

    # Launch the mecanum_parameters_node
    start_mecanum_parameters_node_cmd = Node(
        package=package_name_system_tests,
        executable='mecanum_parameters_node',
        parameters=[mecanum_params],
        output='screen')

    # Create the launch description and populate it
    ld = LaunchDescription()

    # Declare the launch options
    ld.add_action(declare_mecanum_params_cmd)

    # Add actions to the launch description
    ld.add_action(start_mecanum_parameters_node_cmd)

    return ld

# Desired parameter values
/mecanum_parameters_node:
  ros__parameters:
    robot_name: "New Automatic Addison Bot"
    robot_id: 82
    target_linear_velocity_x: 0.0
    target_linear_velocity_y: 0.0
    target_angular_velocity: 0.23

# Copy necessary files to designated locations in the project
install (
  DIRECTORY config launch
  DESTINATION share/${PROJECT_NAME}
)