simulation_benchmark/include/log.hh at gz-sim_dev · yaswanth1701/simulation_benchmark · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
#define MCAP_IMPLEMENTATION
#include "mcap/writer.hpp"
#include "protobuf/BuildFileDescriptorSet.h"
#include <box_msg.pb.h>
#include <iostream>
#include <gz/math/Vector3.hh>
#include <gz/math/Pose3.hh>
#include <gtest/gtest.h>

using  namespace gz;


    template <typename T>
    class Log
    {
    public: explicit Log(const std::string &_filePath)
    {
     auto options = mcap::McapWriterOptions("");
     const auto s = writer.open(_filePath.c_str(), options);
     if(!s.ok())
     {
        std::cout << "file not created" << _filePath << std::endl;
     }
    }

    public: void setBoxMsg(const std::string &_physicsEngine,
               double &_dt, bool &_complex, bool &_collision,
               int &_modelCount, bool &_logMultiple)
    {
      mcap::Schema schema("benchmark_proto.BoxesMsg", "protobuf",
                         foxglove::BuildFileDescriptorSet(benchmark_proto::BoxesMsg::descriptor()).SerializeAsString());

      writer.addSchema(schema);
      mcap::Channel channel("model_states", "protobuf", schema.id);
      writer.addChannel(channel);
      channelId = channel.id;
      msg.set_physics_engine(_physicsEngine);
      msg.set_dt(_dt);
      msg.set_complex(_complex);
      msg.set_collision(_collision);
      msg.set_model_count(_modelCount);
      msg.set_log_multiple(_logMultiple);

      if(_logMultiple)
      {
        for(int model_no = 1; model_no<=_modelCount; model_no++)
        {
          msg.add_data()->set_model_no(model_no);
        }
      }

      else
        msg.add_data()->set_model_no(1);
    }

    public: void stop()
    {
      std::string serialized = msg.SerializeAsString();
      mcap::Message mcapMsg;
      mcapMsg.channelId = channelId;
      mcapMsg.data = reinterpret_cast<const std::byte*>(serialized.data());
      mcapMsg.dataSize = serialized.size();
      const auto res = writer.write(mcapMsg);
    }

    public: void recordSimTime(double &_t)
    {
      msg.add_sim_time(_t);
    }

    public: void recordComputationTime(double &_computationTime)
    {
      msg.set_computation_time(_computationTime);
    }

    public:  void recordPose(int &_modelIdx, const std::vector<double> _position,
                            const std::vector<double> _quaternion)
    {
      auto pose = msg.mutable_data(_modelIdx)->add_poses();

      pose->mutable_position()->set_x(_position[0]);
      pose->mutable_position()->set_y(_position[1]);
      pose->mutable_position()->set_z(_position[3]);

      pose->mutable_orientation()->set_w(_quaternion[0]);
      pose->mutable_orientation()->set_x(_quaternion[1]);
      pose->mutable_orientation()->set_y(_quaternion[2]);
      pose->mutable_orientation()->set_z(_quaternion[3]);
    }

    public: void recordPose(int &_modelIdx, const math::Pose3d &_pose)
    {
      math::Vector3d p = _pose.Pos();
      math::Quaterniond r = _pose.Rot();

      auto pose = msg.mutable_data(_modelIdx)->add_poses();

      pose->mutable_position()->set_x(p.X());
      pose->mutable_position()->set_y(p.Y());
      pose->mutable_position()->set_z(p.Z());

      pose->mutable_orientation()->set_w(r.W());
      pose->mutable_orientation()->set_x(r.X());
      pose->mutable_orientation()->set_y(r.Y());
      pose->mutable_orientation()->set_z(r.Z());
    }

    public: void recordTwist(int &_modelIdx, const std::vector<double> &_linVelocity,
                             const std::vector<double> &_angVelocity)
    {
      auto twist = msg.mutable_data(_modelIdx)->add_twists();

      twist->mutable_linear()->set_x(_linVelocity[0]);
      twist->mutable_linear()->set_y(_linVelocity[1]);
      twist->mutable_linear()->set_z(_linVelocity[2]);

      twist->mutable_angular()->set_x(_angVelocity[0]);
      twist->mutable_angular()->set_y(_angVelocity[1]);
      twist->mutable_angular()->set_z(_angVelocity[2]);
    }

    public: void recordTwist(int &_modelIdx, const math::Vector3d &_linVelocity,
                             const math::Vector3d &_angVelocity)
    {
      auto twist = msg.mutable_data(_modelIdx)->add_twists();

      twist->mutable_linear()->set_x(_linVelocity.X());
      twist->mutable_linear()->set_y(_linVelocity.Y());
      twist->mutable_linear()->set_z(_linVelocity.Z());

      twist->mutable_angular()->set_x(_angVelocity.X());
      twist->mutable_angular()->set_y(_angVelocity.Y());
      twist->mutable_angular()->set_z(_angVelocity.Z());
    }

    private: mcap::McapWriter writer;
    private: mcap::ChannelId channelId;
    private: T msg;
    };