Skip to content

Commit

Permalink
update externalObject
Browse files Browse the repository at this point in the history
  • Loading branch information
@dan-du-car
dan-du-car committed Jul 18, 2023
1 parent 0963f01 commit c3b7ba8
Show file tree
Hide file tree
Showing 5 changed files with 103 additions and 93 deletions.
81 changes: 58 additions & 23 deletions src/tmx/Messages/include/simulation/ExternalObject.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,23 +22,24 @@ namespace tmx
{
public:
ExternalObject(){};
ExternalObject(const tmx::message_container_type &contents) : tmx::message(contents) {}
ExternalObject(PRESENCE_VECTOR_TYPES presenceVector, uint32_t id,
double confidence, OBJECT_TYPES objectType, bool dynamticObj)
{
set_PresenceVector(presenceVector);
set_Id(id);
set_Confidence(confidence);
set_ObjectType(objectType);
set_DynamticObj(dynamticObj);
};
ExternalObject(const tmx::message_container_type &contents) : tmx::message(contents) {};
~ExternalObject(){};
// Message type fpr routing this message through TMX core
static constexpr const char *MessageType = MSGTYPE_DETECTED_STRING;

// Message sub type for routing this message through TMX core
static constexpr const char *MessageSubType = MSGSUBTYPE_EXTERNAL_OBJECT_STRING;

/**
* Metadata to describe the external object
*/
std_attribute(this->msg, bool, MetadataIsSimulation, false, );
std_attribute(this->msg, std::string, MetadataDatum, "", );
std_attribute(this->msg, std::string, MetadataProjString, "", );
std_attribute(this->msg, std::string, MetadataSensorX, "", );
std_attribute(this->msg, std::string, MetadataSensorY, "", );
std_attribute(this->msg, std::string, MetadataSensorZ, "", );
std_attribute(this->msg, std::string, MetadataInfrastructureId, "", );
std_attribute(this->msg, std::string, MetadataSensorId, "", );
/**
*Header contains the frame rest of the fields will use
*/
Expand All @@ -49,9 +50,9 @@ namespace tmx
// # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
// # time-handling sugar is provided by the client library
// The seconds component, valid over all int32 values.
std_attribute(this->msg, uint32_t, HeaderTimeSec, 0, );
std_attribute(this->msg, uint32_t, HeaderStampSecs, 0, );
// # The nanoseconds component, valid in the range [0, 10e9).
std_attribute(this->msg, uint32_t, HeaderTimeNanoSec, 0, );
std_attribute(this->msg, uint32_t, HeaderStampNSecs, 0, );

// A presence vector, this message is used to describe objects coming from potentially different
// sources. The presence vector is used to determine what items are set by the producer.
Expand Down Expand Up @@ -88,27 +89,62 @@ namespace tmx
// Pose of the object within the frame specified in header
// geometry_msgs/PoseWithCovariance pose
// This represents a pose in free space with uncertainty.
std_attribute(this->msg, double, PosePointX, 0, );
std_attribute(this->msg, double, PosePointY, 0, );
std_attribute(this->msg, double, PosePointZ, 0, );
std_attribute(this->msg, double, PosePosePositionX, 0, );
std_attribute(this->msg, double, PosePosePositionY, 0, );
std_attribute(this->msg, double, PosePosePositionZ, 0, );

// This represents an orientation in free space in quaternion form.
std_attribute(this->msg, double, PoseQuaternionX, 0, );
std_attribute(this->msg, double, PoseQuaternionY, 0, );
std_attribute(this->msg, double, PoseQuaternionZ, 0, );
std_attribute(this->msg, double, PoseQuaternionW, 0, );
std_attribute(this->msg, double, PosePoseOrientationX, 0, );
std_attribute(this->msg, double, PosePoseOrientationY, 0, );
std_attribute(this->msg, double, PosePoseOrientationZ, 0, );
std_attribute(this->msg, double, PosePoseOrientationW, 0, );

// Row-major representation of the 6x6 covariance matrix
// # The orientation parameters use a fixed-axis representation.
// # In order, the parameters are:
// # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
std_attribute(this->msg, double, Covariance, 0, );
struct Covariance
{
double covariance = 0;

Covariance() {}
Covariance(double covariance) : covariance(covariance) {}

static message_tree_type to_tree(Covariance element)
{
message_tree_type treeElement;
treeElement.put("Covariance", element.covariance);
return treeElement;
}

static Covariance from_tree(message_tree_type &treeElement)
{
Covariance element;
element.covariance = treeElement.get<std::uint8_t>("Covariance");
return element;
}
};
array_attribute( Covariance, PoseCovariance);

// #Average velocity of the object within the frame specified in header
// geometry_msgs/TwistWithCovariance velocity
std_attribute(this->msg, double, VelocityTwistLinearX, 0, );
std_attribute(this->msg, double, VelocityTwistLinearY, 0, );
std_attribute(this->msg, double, VelocityTwistLinearZ, 0, );
std_attribute(this->msg, double, VelocityTwistAngularX, 0, );
std_attribute(this->msg, double, VelocityTwistAngularY, 0, );
std_attribute(this->msg, double, VelocityTwistAngularZ, 0, );
array_attribute( Covariance, VelocityCovariance);

// #Instantaneous velocity of an object within the frame specified in header
// geometry_msgs/TwistWithCovariance velocity_inst
std_attribute(this->msg, double, VelocityInstTwistLinearX, 0, );
std_attribute(this->msg, double, VelocityInstTwistLinearY, 0, );
std_attribute(this->msg, double, VelocityInstTwistLinearZ, 0, );
std_attribute(this->msg, double, VelocityInstTwistAngularX, 0, );
std_attribute(this->msg, double, VelocityInstTwistAngularY, 0, );
std_attribute(this->msg, double, VelocityInstTwistAngularZ, 0, );
array_attribute( Covariance, VelocityInstCovariance);

// #The size of the object aligned along the axis of the object described by the orientation in pose
// #Dimensions are specified in meters
Expand All @@ -117,8 +153,7 @@ namespace tmx
# It is only meant to represent a direction. Therefore, it does not
# make sense to apply a translation to it (e.g., when applying a
# generic rigid transformation to a Vector3, tf2 will only apply the
# rotation). If you want your data to be translatable too, use the
# geometry_msgs/Point message instead.
# rotation).
*/
// geometry_msgs/Vector3 size
std_attribute(this->msg, double, SizeX, 0, );
Expand Down
7 changes: 4 additions & 3 deletions src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) :
AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage);
AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage);
AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage);
AddMessageFilter < simulation::ExternalObject> (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage );
AddMessageFilter < ExternalObject > (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage );

SubscribeToMessages();

Expand Down Expand Up @@ -628,10 +628,11 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){

}

void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg)
void CARMAStreetsPlugin::HandleSimulatedExternalMessage(ExternalObject &msg, routeable_message &routeableMsg)
{
PLOG(logINFO) << "HandleSimulatedExternalMessage called." <<std::endl;
PLOG(logINFO) << msg.to_string()<<std::endl;
PLOG(logINFO) << "Message Received " << msg.to_string()<<std::endl;
PLOG(logINFO) << "Message Received " << msg.get_PosePosePositionX()<<std::endl;
}

bool CARMAStreetsPlugin::getEncodedtsm3( tsm3EncodedMessage *tsm3EncodedMsg, Json::Value metadata, Json::Value payload_json )
Expand Down
3 changes: 2 additions & 1 deletion src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ using namespace std;
using namespace tmx;
using namespace tmx::utils;
using namespace tmx::messages;
using namespace tmx::messages::simulation;
using namespace boost::property_tree;

namespace CARMAStreetsPlugin {
Expand All @@ -49,7 +50,7 @@ class CARMAStreetsPlugin: public PluginClient {
void HandleMobilityOperationMessage(tsm3Message &msg, routeable_message &routeableMsg);
void HandleMobilityPathMessage(tsm2Message &msg, routeable_message &routeableMsg);
void HandleBasicSafetyMessage(BsmMessage &msg, routeable_message &routeableMsg);
void HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg);
void HandleSimulatedExternalMessage(ExternalObject &msg, routeable_message &routeableMsg);
/**
* @brief Subscribe to MAP message broadcast by the MAPPlugin. This handler will be called automatically whenever the MAPPlugin is broadcasting a J2735 MAP message.
* @param msg The J2735 MAP message received from the internal
Expand Down
101 changes: 38 additions & 63 deletions src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,80 +13,55 @@
count_num = 0

def generate_sim_external_object():
jsonResult = {
"metadata":{
"is_simulation": False,
"datum": "",
"proj_string": "",
"sensor_x": 0.0,
"sensor_y": 0.0,
"sensor_z": 0.0,
"infrastructure_id": "",
"sensor_id": ""
},
"header": {
"seq": 0,
"stamp": {
"secs": 0,
"nsecs": 0
}
},
"id": 0,
"pose": {
"pose": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"w": 0.0
}
},
"covariance": [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
jsonResult ={
"MetadataIsSimulation": False,
"MetadataDatum": "",
"MetadataProjString": "",
"MetadataSensorX": "",
"MetadataSensorY": "",
"MetadataSensorZ": "",
"MetadataInfrastructureId": "",
"MetadataSensorId": "",
"HeaderStampSecs": 1,
"HeaderStampNsecs": 2,
"Id": 10,
"PosePosePositionX": 0.2,
"PosePosePositionY": 0.3,
"PosePosePositionZ": 0.4,
"PosePoseOrientationX": 0.1,
"PosePoseOrientationY": 0.2,
"PosePoseOrientationZ": 0.3,
"PosePoseOrientationW": 0.4,
"BsmId": [12,12,22,11],
"PoseCovariance": [
1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
},
"velocity": {
"twist": {
"linear": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"angular": {
"x": 0.0,
"y": 0.0,
"z": 0.0
}
},
"covariance": [
],
"VelocityTwistLinearX": 0.1,
"VelocityTwistLinearY": 0.1,
"VelocityTwistLinearZ": 0.1,
"VelocityTwistAngularX": 0.1,
"VelocityTwistAngularY": 0.1,
"VelocityTwistAngularZ": 0.1,
"VelocityCovariance": [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
},
"size": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"confidence": 0.0,
"object_type": "",
"dynamic_obj": False
}

],
"SizeX": 0.3,
"SizeY": 0.4,
"SizeZ": 0.6,
"Confidence": 0.0,
"ObjectType": "",
"DynamicObj": False
}
jsonResult = json.dumps(jsonResult)
return jsonResult
port = 7576
Expand Down
4 changes: 1 addition & 3 deletions src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,10 +174,8 @@ namespace CDASimAdapter{
external_bject_detection_thread_timer->AddPeriodicTick([this](){
PLOG(logDEBUG1) << "Listening for External Object Message from CDASim." << std::endl;
auto msg = connection->consume_external_object_message();
if ( !msg.is_empty()) {
PLOG(logDEBUG1) << "Consumed External Object Message: " << msg<<std::endl;
if ( !msg.is_empty()) {
this->forward_simulated_external_message(msg);
PLOG(logDEBUG1) << "External Object Message Forwarded to CARMAStreetsPlugin!" << std::endl;
}
else
{
Expand Down

0 comments on commit c3b7ba8

Please sign in to comment.