Skip to content

Commit

Permalink
PR Updates
Browse files Browse the repository at this point in the history
  • Loading branch information
@paulbourelly999
paulbourelly999 committed Dec 17, 2023
1 parent edf0028 commit b93fab0
Show file tree
Hide file tree
Showing 3 changed files with 106 additions and 10 deletions.
34 changes: 34 additions & 0 deletions src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,6 +193,12 @@ namespace CARMAStreetsPlugin
auto acc_cfd_yaw = (AccelerationConfidence_t*)calloc(1, sizeof(YawRateConfidence_t));
*acc_cfd_yaw = (*itr)["detected_object_data"]["detected_object_common_data"]["acc_cfd_yaw"].asInt64();
objectData->detObjCommon.accCfdYaw = acc_cfd_yaw;
}
// Object Optional Data
if ((*itr)["detected_object_data"].isMember("detected_object_optional_data") ){
auto optional_data = (DetectedObjectOptionalData_t*)calloc(1, sizeof(DetectedObjectOptionalData_t));
populateOptionalData((*itr)["detected_object_data"]["detected_object_optional_data"], optional_data);
objectData->detObjOptData = optional_data;
}
ASN_SEQUENCE_ADD(&objects->list.array, objectData);
}
Expand All @@ -214,5 +220,33 @@ namespace CARMAStreetsPlugin
delete(_sdsmMessage);
}

void JsonToJ3224SDSMConverter::populateOptionalData(const Json::Value &optional_data_json, DetectedObjectOptionalData_t *optional_data) const {
if ( optional_data_json.isMember("detected_vehicle_data") ) {
optional_data->present = DetectedObjectOptionalData_PR_detVeh;
// Optional Vehicle Attitude
if (optional_data_json["detected_vehicle_data"].isMember("veh_attitude")) {
auto attitude = (Attitude_t*) calloc( 1, sizeof(Attitude_t));
attitude->pitch = optional_data_json["detected_vehicle_data"]["veh_attitude"]["pitch"].asInt64();
attitude->roll = optional_data_json["detected_vehicle_data"]["veh_attitude"]["roll"].asInt64();
attitude->yaw = optional_data_json["detected_vehicle_data"]["veh_attitude"]["yaw"].asInt64();
optional_data->choice.detVeh.vehAttitude = attitude;
}
// Optional Vehicle Attitude Confidence
if (optional_data_json["detected_vehicle_data"].isMember("veh_attitude_confidence")) {
auto attitude_confidence = (AttitudeConfidence_t*) calloc( 1, sizeof(AttitudeConfidence_t));
attitude_confidence->pitchConfidence = optional_data_json["detected_vehicle_data"]["veh_attitude_confidence"]["pitch_confidence"].asInt64();
attitude_confidence->rollConfidence = optional_data_json["detected_vehicle_data"]["veh_attitude_confidence"]["roll_confidence"].asInt64();
attitude_confidence->yawConfidence = optional_data_json["detected_vehicle_data"]["veh_attitude_confidence"]["yaw_confidence"].asInt64();
optional_data->choice.detVeh.vehAttitudeConfidence = attitude_confidence;
}
// Optional Vehicle Angular Velocity
if (optional_data_json["detected_vehicle_data"].isMember("veh_ang_vel")) {
auto angular_velocity = (AngularVelocity_t*) calloc( 1, sizeof(AngularVelocity_t));
angular_velocity->pitchRate = optional_data_json["detected_vehicle_data"]["veh_ang_vel"]["pitch_rate"].asInt64();
angular_velocity->rollRate = optional_data_json["detected_vehicle_data"]["veh_ang_vel"]["roll_rate"].asInt64();
optional_data->choice.detVeh.vehAngVel = angular_velocity;
}
}
}

}
4 changes: 3 additions & 1 deletion src/v2i-hub/CARMAStreetsPlugin/src/JsonToJ3224SDSMConverter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,10 @@ namespace CARMAStreetsPlugin
* @param Encoded J3224 SDSM
*/
void encodeSDSM(const std::shared_ptr<SensorDataSharingMessage_t> &sdsmPtr, tmx::messages::SdsmEncodedMessage &encodedSDSM) const;

~JsonToJ3224SDSMConverter() = default;
private:
void populateOptionalData(const Json::Value &optional_data_json, DetectedObjectOptionalData_t *optional_data) const;
};

}
78 changes: 69 additions & 9 deletions src/v2i-hub/CARMAStreetsPlugin/test/test_JsonToJ3224SDSMConverter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,24 +163,84 @@ namespace CARMAStreetsPlugin
yaw":400
},
"heading":16000,
"heading_conf":4,"measurement_time":-1100,"object_id":12200,"obj_type":1,"obj_type_cfd":65,"pos":{"offset_x":4000,"offset_y":-720,"offset_z":20},"pos_confidence":{"elevation":5,"pos":2},"speed":2100,"speed_confidence":3,"speed_confidence_z":4,"speed_z":1000,"time_confidence":2},"detected_object_optional_data":{"detected_vehicle_data":{"height":70,"lights":8,"size":{"length":700,"width":300},"veh_ang_vel":{"pitch_rate":600,"roll_rate":-800},"veh_ang_vel_confidence":{"pitch_rate_confidence":3,"roll_rate_confidence":4},"veh_attitude":{"pitch":2400,"roll":-12000,"yaw":400},"veh_attitude_confidence":{"pitch_confidence":2,"roll_confidence":3,"yaw_confidence":4},"vehicle_class":11,"vehicle_class_conf":75,"vehicle_size_confidence":{"vehicle_height_confidence":5,"vehicle_length_confidence":6,"vehicle_width_confidence":7}}}}}],"ref_pos":{"long":600000000,"elevation":30,"lat":400000000},"ref_pos_el_conf":10,"ref_pos_xy_conf":{"orientation":25000,"semi_major":235,"semi_minor":200},"sdsm_time_stamp":{"day":4,"hour":19,"minute":15,"month":7,"offset":400,"second":5000,"year":2007},"source_id":"01020304"})";
"heading_conf":4,
"measurement_time":-1100,
"object_id":12200,
"obj_type":1,
"obj_type_cfd":65,
"pos":{"offset_x":4000,"offset_y":-720,"offset_z":20},
"pos_confidence":{"elevation":5,"pos":2},
"speed":2100,
"speed_confidence":3,
"speed_confidence_z":4,
"speed_z":1000,
"time_confidence":2
},
"detected_object_optional_data":{
"detected_vehicle_data":{
"height":70,
"lights":8,
"size":{
"length":700,
"width":300
},
"veh_ang_vel":{
"pitch_rate":600,
"roll_rate":-800
},
"veh_ang_vel_confidence":{
"pitch_rate_confidence":3,
"roll_rate_confidence":4
},
"veh_attitude":{
"pitch":2400,
"roll":-12000,
"yaw":400
},
"veh_attitude_confidence":{
"pitch_confidence":2,
"roll_confidence":3,
"yaw_confidence":4
},
"vehicle_class":11,
"vehicle_class_conf":75,
"vehicle_size_confidence":{
"vehicle_height_confidence":5,
"vehicle_length_confidence":6,
"vehicle_width_confidence":7
}
}
}
}
}
],
"ref_pos":{
"long":600000000,
"elevation":30,
"lat":400000000
},
"ref_pos_el_conf":10,
"ref_pos_xy_conf":{"orientation":25000,"semi_major":235,"semi_minor":200},
"sdsm_time_stamp":{"day":4,"hour":19,"minute":15,"month":7,"offset":400,"second":5000,"year":2007},
"source_id":"01020304"
})";
Json::Value root;
bool result = converter.parseJsonString(valid_json_str, root);
ASSERT_TRUE(result);
auto sdsmPtr = std::make_shared<SensorDataSharingMessage>();
converter.convertJsonToSDSM(root, sdsmPtr);


// ASSERT_EQ(2400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->pitch);
// ASSERT_EQ(-12000, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->roll);
// ASSERT_EQ(400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->yaw);
ASSERT_EQ(2400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->pitch);
ASSERT_EQ(-12000, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->roll);
ASSERT_EQ(400, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitude->yaw);

// ASSERT_EQ(2, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->pitchConfidence);
// ASSERT_EQ(3, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->rollConfidence);
// ASSERT_EQ(4, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->yawConfidence);
ASSERT_EQ(2, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->pitchConfidence);
ASSERT_EQ(3, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->rollConfidence);
ASSERT_EQ(4, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAttitudeConfidence->yawConfidence);

// ASSERT_EQ(600, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVel->pitchRate);
// ASSERT_EQ(-800, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVel->rollRate);
ASSERT_EQ(600, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVel->pitchRate);
ASSERT_EQ(-800, sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVel->rollRate);

// ASSERT_EQ(3, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVelConfidence->pitchRateConfidence);
// ASSERT_EQ(4, *sdsmPtr->objects.list.array[0]->detObjOptData->choice.detVeh.vehAngVelConfidence->rollRateConfidence);
Expand Down

0 comments on commit b93fab0

Please sign in to comment.