Skip to content

Commit

Permalink
address comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@dan-du-car
dan-du-car committed Aug 12, 2024
1 parent 78f4bb6 commit 3914330
Show file tree
Hide file tree
Showing 8 changed files with 190 additions and 130 deletions.
23 changes: 23 additions & 0 deletions src/tmx/Messages/include/Covariance.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
#pragma once
#include <tmx/messages/message.hpp>
namespace tmx
{
namespace messages
{
typedef struct Covariance{
std::string value;
Covariance(){};
Covariance( std::string value):value(value){};
static message_tree_type to_tree(const Covariance& cov){
message_tree_type tree;
tree.put("", cov.value);
return tree;
}
static Covariance from_tree(const message_tree_type& tree){
Covariance cov;
cov.value = tree.get<std::string>("");
return cov;
}
} Covariance;
}
}
28 changes: 28 additions & 0 deletions src/tmx/Messages/include/Position.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
#pragma once
#include <tmx/messages/message.hpp>
namespace tmx
{
namespace messages
{
// Cartesian positiion of object. Assumed to be ENU coordinate frame.
typedef struct Position{
double x, y, z;
Position(){};
Position(double x, double y, double z):x(x),y(y),z(z){};
static message_tree_type to_tree(const Position& pos){
message_tree_type tree;
tree.put("x", pos.x);
tree.put("y", pos.y);
tree.put("z", pos.z);
return tree;
}
static Position from_tree(const message_tree_type& tree){
Position pos;
pos.x = tree.get<double>("x");
pos.y = tree.get<double>("y");
pos.z = tree.get<double>("z");
return pos;
}
} Position;
}
}
90 changes: 7 additions & 83 deletions src/tmx/Messages/include/SensorDetectedObject.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,10 @@
#include <MessageTypes.h>
#include <Vector3d.h>
#include <Point.h>
#include "Position.h"
#include "Covariance.h"
#include "Velocity.h"
#include "Size.h"

namespace tmx
{
Expand Down Expand Up @@ -41,101 +45,21 @@ namespace tmx
// Unique identifier of detected object.
std_attribute(this->msg, int, ObjectId, 0, );

// Cartesian positiion of object. Assumed to be ENU coordinate frame.
typedef struct Position{
double x, y, z;
Position(){};
Position(double x, double y, double z):x(x),y(y),z(z){};
static message_tree_type to_tree(const Position& pos){
message_tree_type tree;
tree.put("x", pos.x);
tree.put("y", pos.y);
tree.put("z", pos.z);
return tree;
}
static Position from_tree(const message_tree_type& tree){
Position pos;
pos.x = tree.get<double>("x");
pos.y = tree.get<double>("y");
pos.z = tree.get<double>("z");
return pos;
}
} Position;
object_attribute(Position, Position);

typedef struct Covariance{
std::string value;
Covariance(){};
Covariance( std::string value):value(value){};
static message_tree_type to_tree(const Covariance& cov){
message_tree_type tree;
tree.put("", cov.value);
return tree;
}
static Covariance from_tree(const message_tree_type& tree){
Covariance cov;
cov.value = tree.get<std::string>("");
return cov;
}
} Covariance;

object_attribute(Position, Position);
array_attribute(Covariance, PositionCovariance);

// Cartesian velocity vector of object. Assumed to be ENU coordinate frame.
typedef struct Velocity{
double x, y, z;
Velocity(){};
Velocity(double x, double y, double z):x(x),y(y),z(z){};
static message_tree_type to_tree(const Velocity& velocity){
message_tree_type tree;
tree.put("x", velocity.x);
tree.put("y", velocity.y);
tree.put("z", velocity.z);
return tree;
}
static Velocity from_tree(const message_tree_type& tree){
Velocity velocity;
velocity.x = tree.get<double>("x");
velocity.y = tree.get<double>("y");
velocity.z = tree.get<double>("z");
return velocity;
}
} Velocity;
//Linear velocity in meter per second
object_attribute(Velocity, Velocity);
//Covariance associated with linear velocity.
array_attribute(Covariance, VelocityCovariance);

//Angular velocity in radians per second.
object_attribute(Velocity, AngularVelocity);
//Covariance associated with angular velocity.
array_attribute(Covariance, AngularVelocityCovariance);

// Epoch time in milliseconds.
// long timestamp = 0;
std_attribute(this->msg, long, Timestamp, 0, );

//Length, width and height of object in meter.
typedef struct Size{
double length;
double width;
double height;
Size(){};
Size(double length, double width, double height): length(length), width(width), height(height){};
static message_tree_type to_tree(const Size& size){
message_tree_type tree;
tree.put("length", size.length);
tree.put("width", size.width);
tree.put("height", size.height);
return tree;
}
static Size from_tree(const message_tree_type & tree){
Size size;
size.length = tree.get<double>("length");
size.width = tree.get<double>("width");
size.height = tree.get<double>("height");
return size;
}
} Size;
std_attribute(this->msg, long, Timestamp, 0, );
object_attribute(Size, Size);

};
Expand Down
30 changes: 30 additions & 0 deletions src/tmx/Messages/include/Size.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
#pragma once
#include <tmx/messages/message.hpp>
namespace tmx
{
namespace messages
{
//Length, width and height of object in meter.
typedef struct Size{
double length;
double width;
double height;
Size(){};
Size(double length, double width, double height): length(length), width(width), height(height){};
static message_tree_type to_tree(const Size& size){
message_tree_type tree;
tree.put("length", size.length);
tree.put("width", size.width);
tree.put("height", size.height);
return tree;
}
static Size from_tree(const message_tree_type & tree){
Size size;
size.length = tree.get<double>("length");
size.width = tree.get<double>("width");
size.height = tree.get<double>("height");
return size;
}
} Size;
}
}
28 changes: 28 additions & 0 deletions src/tmx/Messages/include/Velocity.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
#pragma once
#include <tmx/messages/message.hpp>
namespace tmx
{
namespace messages
{
// Cartesian velocity vector of object. Assumed to be ENU coordinate frame.
typedef struct Velocity{
double x, y, z;
Velocity(){};
Velocity(double x, double y, double z):x(x),y(y),z(z){};
static message_tree_type to_tree(const Velocity& velocity){
message_tree_type tree;
tree.put("x", velocity.x);
tree.put("y", velocity.y);
tree.put("z", velocity.z);
return tree;
}
static Velocity from_tree(const message_tree_type& tree){
Velocity velocity;
velocity.x = tree.get<double>("x");
velocity.y = tree.get<double>("y");
velocity.z = tree.get<double>("z");
return velocity;
}
} Velocity;
}
}
113 changes: 70 additions & 43 deletions src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,52 +1,79 @@
#include <gtest/gtest.h>
#include "SensorDetectedObject.h"

namespace tmx::messages{
TEST(SensorDetectedObjectTest, attributes){
SensorDetectedObject tmxSdsm;
tmxSdsm.set_ISSimulated(false);
SensorDetectedObject::Position pos(1.0, 2.3, 2.0);
tmxSdsm.set_Position(pos);
class SensorDetectedObjectTest : public testing::Test{
protected:
std::shared_ptr<SensorDetectedObject> tmxSdsmPtr;
SensorDetectedObjectTest(){
tmxSdsmPtr = std::make_shared<SensorDetectedObject>();
}
void SetUp() override {
tmxSdsmPtr->set_ISSimulated(false);
Position pos(1.0, 2.3, 2.0);
tmxSdsmPtr->set_Position(pos);
tmxSdsmPtr->set_ProjString("+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs");
tmxSdsmPtr->set_Timestamp(12222222222);
tmxSdsmPtr->set_SensorId("SomeID");
tmxSdsmPtr->set_Type("Car");
tmxSdsmPtr->set_Confidence(0.7);
tmxSdsmPtr->set_ObjectId(123);

tmxSdsm.set_ProjString("+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs");
tmxSdsm.set_Timestamp(12222222222);
tmxSdsm.set_SensorId("SomeID");
tmxSdsm.set_Type("Car");
tmxSdsm.set_Confidence(0.7);
tmxSdsm.set_ObjectId(123);
Velocity vel(1.0, 0.3, 2.0);
tmxSdsmPtr->set_Velocity(vel);
tmxSdsmPtr->set_AngularVelocity(vel);

SensorDetectedObject::Velocity vel(1.0, 0.3, 2.0);
tmxSdsm.set_Velocity(vel);
tmxSdsm.set_AngularVelocity(vel);
std::vector<Covariance> covs {
Covariance("a11"),
Covariance("a12"),
Covariance("a13"),
Covariance("a21"),
Covariance("a22"),
Covariance("a23"),
Covariance("a31"),
Covariance("a32"),
Covariance("a33"),
Covariance("a41")};
tmxSdsmPtr->set_PositionCovariance(covs);
tmxSdsmPtr->set_VelocityCovariance(covs);
tmxSdsmPtr->set_AngularVelocityCovariance(covs);
}
};

std::vector<SensorDetectedObject::Covariance> covs {
SensorDetectedObject::Covariance("a11"),
SensorDetectedObject::Covariance("a12"),
SensorDetectedObject::Covariance("a13"),
SensorDetectedObject::Covariance("a21"),
SensorDetectedObject::Covariance("a22"),
SensorDetectedObject::Covariance("a23"),
SensorDetectedObject::Covariance("a31"),
SensorDetectedObject::Covariance("a32"),
SensorDetectedObject::Covariance("a33"),
SensorDetectedObject::Covariance("a41")};
tmxSdsm.set_PositionCovariance(covs);
tmxSdsm.set_VelocityCovariance(covs);
tmxSdsm.set_AngularVelocityCovariance(covs);
TEST_F(SensorDetectedObjectTest, attributes){
EXPECT_EQ(false, tmxSdsmPtr->get_ISSimulated());
EXPECT_EQ(0.7, tmxSdsmPtr->get_Confidence());
EXPECT_EQ("SomeID", tmxSdsmPtr->get_SensorId());
EXPECT_EQ(12222222222, tmxSdsmPtr->get_Timestamp());
EXPECT_EQ(123, tmxSdsmPtr->get_ObjectId());
EXPECT_EQ("+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs", tmxSdsmPtr->get_ProjString());
EXPECT_EQ(1.0, tmxSdsmPtr->get_Position().x);
EXPECT_NEAR(2.3, tmxSdsmPtr->get_Position().y, 0.01);
EXPECT_EQ(2.0, tmxSdsmPtr->get_Position().z);
EXPECT_EQ(1.0, tmxSdsmPtr->get_Velocity().x);
EXPECT_NEAR(0.3, tmxSdsmPtr->get_Velocity().y, 0.01);
EXPECT_EQ(2.0, tmxSdsmPtr->get_Velocity().z);
EXPECT_EQ(1.0, tmxSdsmPtr->get_AngularVelocity().x);
EXPECT_NEAR(0.3, tmxSdsmPtr->get_AngularVelocity().y, 0.01);
EXPECT_EQ(2.0, tmxSdsmPtr->get_AngularVelocity().z);
EXPECT_EQ(10, tmxSdsmPtr->get_PositionCovariance().size());
EXPECT_EQ(10, tmxSdsmPtr->get_AngularVelocityCovariance().size());
EXPECT_EQ(10, tmxSdsmPtr->get_VelocityCovariance().size());
}

TEST_F(SensorDetectedObjectTest, to_string){
std::string expectedStr = "{\"ISSimulated\":\"0\",\"Position\":{\"x\":\"1\",\"y\":\"2.2999999999999998\",\"z\":\"2\"},\"ProjString\":\"+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs\",\"Timestamp\":\"12222222222\",\"SensorId\":\"SomeID\",\"Type\":\"Car\",\"Confidence\":\"0.69999999999999996\",\"ObjectId\":\"123\",\"Velocity\":{\"x\":\"1\",\"y\":\"0.29999999999999999\",\"z\":\"2\"},\"AngularVelocity\":{\"x\":\"1\",\"y\":\"0.29999999999999999\",\"z\":\"2\"},\"PositionCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"],\"VelocityCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"],\"AngularVelocityCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"]}\n";
EXPECT_EQ(expectedStr, tmxSdsmPtr->to_string());
}

EXPECT_EQ(false, tmxSdsm.get_ISSimulated());
EXPECT_EQ(0.7, tmxSdsm.get_Confidence());
EXPECT_EQ("SomeID", tmxSdsm.get_SensorId());
EXPECT_EQ(12222222222, tmxSdsm.get_Timestamp());
EXPECT_EQ(123, tmxSdsm.get_ObjectId());
EXPECT_EQ("+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs", tmxSdsm.get_ProjString());
EXPECT_EQ(1.0, tmxSdsm.get_Velocity().x);
EXPECT_NEAR(0.3, tmxSdsm.get_Velocity().y, 0.01);
EXPECT_EQ(2.0, tmxSdsm.get_Velocity().z);
EXPECT_EQ(1.0, tmxSdsm.get_AngularVelocity().x);
EXPECT_NEAR(0.3, tmxSdsm.get_AngularVelocity().y, 0.01);
EXPECT_EQ(2.0, tmxSdsm.get_AngularVelocity().z);
EXPECT_EQ(10, tmxSdsm.get_PositionCovariance().size());
EXPECT_EQ(10, tmxSdsm.get_AngularVelocityCovariance().size());
EXPECT_EQ(10, tmxSdsm.get_VelocityCovariance().size());
TEST_F(SensorDetectedObjectTest, deserialize){
auto tmxSdsmPtr2 = std::make_shared<SensorDetectedObject>();
std::string expectedStr = "{\"ISSimulated\":\"1\",\"Position\":{\"x\":\"1\",\"y\":\"2.5\",\"z\":\"2\"},\"ProjString\":\"+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs\",\"Timestamp\":\"12222222222\",\"SensorId\":\"SomeID\",\"Type\":\"Car\",\"Confidence\":\"0.7\",\"ObjectId\":\"123\",\"Velocity\":{\"x\":\"1\",\"y\":\"0.5\",\"z\":\"2\"},\"AngularVelocity\":{\"x\":\"1\",\"y\":\"0.3\",\"z\":\"2\"},\"PositionCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"],\"VelocityCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"],\"AngularVelocityCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"]}\n";
tmxSdsmPtr2->set_contents(expectedStr);
EXPECT_EQ(expectedStr, tmxSdsmPtr2->to_string());
EXPECT_EQ(true, tmxSdsmPtr2->get_ISSimulated());
EXPECT_EQ(1.0, tmxSdsmPtr2->get_Position().x);
EXPECT_NEAR(2.5, tmxSdsmPtr2->get_Position().y, 0.01);
EXPECT_EQ(2.0, tmxSdsmPtr2->get_Position().z);
}
}
6 changes: 3 additions & 3 deletions src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ namespace MUSTSensorDriverPlugin {
tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString) {
tmx::messages::SensorDetectedObject detectedObject;
detectedObject.set_ObjectId(detection.trackID);
tmx::messages::SensorDetectedObject::Position pos(detection.position_x, detection.position_y, 0);
tmx::messages::Position pos(detection.position_x, detection.position_y, 0);
detectedObject.set_Position(pos);
detectedObject.set_Confidence(detection.confidence);
detectedObject.set_Timestamp(static_cast<long>(detection.timestamp*1000)); // convert decimal seconds to int milliseconds.
Expand Down Expand Up @@ -77,12 +77,12 @@ namespace MUSTSensorDriverPlugin {
}
};

tmx::messages::SensorDetectedObject::Velocity headingSpeedToVelocity(double heading, double speed) {
tmx::messages::Velocity headingSpeedToVelocity(double heading, double speed) {
// Convert North East heading to Angle with 0 at (1, 0) (See README Unit Circle)
heading = heading - 270;
// factor for converting heading from degrees to radians
auto headingInRad = M_PI / 180;
tmx::messages::SensorDetectedObject::Velocity velocity;
tmx::messages::Velocity velocity;
velocity.x = std::cos(headingInRad * heading) * speed;
velocity.y = std::sin(headingInRad * heading) * speed;
velocity.z = 0;
Expand Down
2 changes: 1 addition & 1 deletion src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,5 +118,5 @@ namespace MUSTSensorDriverPlugin {
* @param speed double speed in m/s
* @return tmx::utils::Vector3d velocity.
*/
tmx::messages::SensorDetectedObject::Velocity headingSpeedToVelocity(double heading, double speed);
tmx::messages::Velocity headingSpeedToVelocity(double heading, double speed);
}

0 comments on commit 3914330

Please sign in to comment.