#!/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;
}