<commit_before>#pragma once

#include <geometry_msgs/msg/quaternion.hpp>
#include "ros2_parser.h"

class QuaternionMsgParser : public BuiltinMessageParser<geometry_msgs::msg::Quaternion>
{
public:
  QuaternionMsgParser(const std::string& topic_name, PlotDataMapRef& plot_data)
    : BuiltinMessageParser<geometry_msgs::msg::Quaternion>(topic_name, plot_data)
  {
    _data.push_back(&getSeries(plot_data, topic_name + "/x"));
    _data.push_back(&getSeries(plot_data, topic_name + "/y"));
    _data.push_back(&getSeries(plot_data, topic_name + "/z"));
    _data.push_back(&getSeries(plot_data, topic_name + "/w"));

    _data.push_back(&getSeries(plot_data, topic_name + "/roll_deg"));
    _data.push_back(&getSeries(plot_data, topic_name + "/pitch_deg"));
    _data.push_back(&getSeries(plot_data, topic_name + "/yaw_deg"));
  }

  void parseMessageImpl(const geometry_msgs::msg::Quaternion& msg, double timestamp) override
  {
    _data[0]->pushBack({ timestamp, msg.x });
    _data[1]->pushBack({ timestamp, msg.y });
    _data[2]->pushBack({ timestamp, msg.z });
    _data[3]->pushBack({ timestamp, msg.w });

    //-----------------------------
    auto q = msg;
    double quat_norm2 = q.w * q.w + q.x * q.x * q.y * q.y + q.z * q.z;
    if (std::abs(quat_norm2 - 1.0) > std::numeric_limits<double>::epsilon())
    {
      double mult = 1.0 / std::sqrt(quat_norm2);
      q.x *= mult;
      q.y *= mult;
      q.z *= mult;
      q.w *= mult;
    }

    double roll, pitch, yaw;
    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
    {
      pitch = std::copysign(M_PI_2, sinp);  // use 90 degrees if out of range
    }
    else<commit_msg>bug fix<commit_after>#pragma once

#include <geometry_msgs/msg/quaternion.hpp>
#include "ros2_parser.h"

class QuaternionMsgParser : public BuiltinMessageParser<geometry_msgs::msg::Quaternion>
{
public:
  QuaternionMsgParser(const std::string& topic_name, PlotDataMapRef& plot_data)
    : BuiltinMessageParser<geometry_msgs::msg::Quaternion>(topic_name, plot_data)
  {
    _data.push_back(&getSeries(plot_data, topic_name + "/x"));
    _data.push_back(&getSeries(plot_data, topic_name + "/y"));
    _data.push_back(&getSeries(plot_data, topic_name + "/z"));
    _data.push_back(&getSeries(plot_data, topic_name + "/w"));

    _data.push_back(&getSeries(plot_data, topic_name + "/roll_deg"));
    _data.push_back(&getSeries(plot_data, topic_name + "/pitch_deg"));
    _data.push_back(&getSeries(plot_data, topic_name + "/yaw_deg"));
  }

  void parseMessageImpl(const geometry_msgs::msg::Quaternion& msg, double timestamp) override
  {
    _data[0]->pushBack({ timestamp, msg.x });
    _data[1]->pushBack({ timestamp, msg.y });
    _data[2]->pushBack({ timestamp, msg.z });
    _data[3]->pushBack({ timestamp, msg.w });

    //-----------------------------
    auto q = msg;
    double quat_norm2 = (q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z);
    if (std::abs(quat_norm2 - 1.0) > std::numeric_limits<double>::epsilon())
    {
      double mult = 1.0 / std::sqrt(quat_norm2);
      q.x *= mult;
      q.y *= mult;
      q.z *= mult;
      q.w *= mult;
    }

    double roll, pitch, yaw;
    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
    {
      pitch = std::copysign(M_PI_2, sinp);  // use 90 degrees if out of range
    }
    else
{'fraction_non_alphanumeric': 0.18673740053050397, 'fraction_numerical': 0.011140583554376658, 'mean_word_length': 3.081168831168831, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 23, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2354338', 'n_tokens_mistral': 1465, 'n_tokens_neox': 1434, 'n_words': 272}