Skip to content

Commit 4fbe84e

Browse files
urfeexurfeex
committed
Add test for IMMI to string
1 parent a5b3438 commit 4fbe84e

2 files changed

Lines changed: 52 additions & 33 deletions

File tree

‎src/primary/robot_state/masterboard_data.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -111,6 +111,7 @@ bool MasterboardData::consumeWith(AbstractPrimaryConsumer& consumer)
111111
std::string MasterboardData::toString() const
112112
{
113113
std::stringstream os;
114+
os << std::boolalpha;
114115
os << "MasterboardData:" << std::endl;
115116
os << "Digital input bits: 0b" << digital_input_bits_ << std::endl;
116117
os << "Digital output bits: 0b" << digital_output_bits_ << std::endl;

‎tests/test_primary_parser.cpp‎

Lines changed: 51 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -44,13 +44,14 @@ using namespace urcl;
4444
namespace
4545
{
4646
// ConfigurationData payload before optional int16 reserved fields (older UR software).
47-
constexpr size_t kConfigurationDataPayloadBytes = 6 * 2 * sizeof(double) + // joint position limits
48-
6 * 2 * sizeof(double) + // joint motion limits
49-
5 * sizeof(double) + // v/a joint/tool defaults + eq_radius
50-
4 * 6 * sizeof(double) + // dh_a, dh_d, dh_alpha, dh_theta
51-
4 * sizeof(int32_t); // masterboard, controller, robot type, sub type
47+
constexpr size_t CONFIGURATION_DATA_PAYLOAD_BYTES =
48+
6 * 2 * sizeof(double) + // joint position limits
49+
6 * 2 * sizeof(double) + // joint motion limits
50+
5 * sizeof(double) + // v/a joint/tool defaults + eq_radius
51+
4 * 6 * sizeof(double) + // dh_a, dh_d, dh_alpha, dh_theta
52+
4 * sizeof(int32_t); // masterboard, controller, robot type, sub type
5253

53-
static_assert(kConfigurationDataPayloadBytes == 440);
54+
static_assert(CONFIGURATION_DATA_PAYLOAD_BYTES == 440);
5455

5556
std::vector<uint8_t> makeRobotStatePacketWithConfigurationSubmessage(const std::vector<uint8_t>& configuration_payload)
5657
{
@@ -79,20 +80,20 @@ std::vector<uint8_t> makeRobotStatePacketWithConfigurationSubmessage(const std::
7980

8081
// Masterboard payload sizes (not including packageSize + packageType header).
8182
// Base payload up to and including `immiInterfaceInstalled` flag.
82-
constexpr size_t kMasterboardPayloadBytesBeforeImmi = 2 * sizeof(uint32_t) + // digital input/output bits
83-
2 * sizeof(uint8_t) + // analog input ranges
84-
2 * sizeof(double) + // analog inputs
85-
2 * sizeof(char) + // analog output domains
86-
2 * sizeof(double) + // analog outputs
87-
4 * sizeof(float) + // temperature + voltages + currents
88-
sizeof(uint8_t) + // safety mode
89-
sizeof(uint8_t) + // in reduced mode (bool)
90-
sizeof(uint8_t); // immi interface installed (bool)
91-
constexpr size_t kMasterboardOptionalImmiBytes = 2 * sizeof(uint32_t) + 2 * sizeof(float);
92-
constexpr size_t kMasterboardPayloadBytesAfterImmi = sizeof(uint32_t) + // reserved
93-
sizeof(uint8_t) + // operational mode selector input
94-
sizeof(uint8_t) + // three position enabling device (bool)
95-
sizeof(uint8_t); // reserved
83+
constexpr size_t MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI = 2 * sizeof(uint32_t) + // digital input/output bits
84+
2 * sizeof(uint8_t) + // analog input ranges
85+
2 * sizeof(double) + // analog inputs
86+
2 * sizeof(char) + // analog output domains
87+
2 * sizeof(double) + // analog outputs
88+
4 * sizeof(float) + // temperature + voltages + currents
89+
sizeof(uint8_t) + // safety mode
90+
sizeof(uint8_t) + // in reduced mode (bool)
91+
sizeof(uint8_t); // immi interface installed (bool)
92+
constexpr size_t MASTERBOARD_OPTIONAL_IMMI_BYTES = 2 * sizeof(uint32_t) + 2 * sizeof(float);
93+
constexpr size_t MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI = sizeof(uint32_t) + // reserved
94+
sizeof(uint8_t) + // operational mode selector input
95+
sizeof(uint8_t) + // three position enabling device (bool)
96+
sizeof(uint8_t); // reserved
9697

9798
std::vector<uint8_t> makeRobotStatePacketWithMasterboardSubmessage(const std::vector<uint8_t>& masterboard_payload)
9899
{
@@ -611,7 +612,7 @@ TEST_F(PrimaryParserTest, parse_safetymode_msg)
611612
}
612613
}
613614

614-
std::string construct_string(int data_type, std::string data)
615+
std::string constructString(int data_type, std::string data)
615616
{
616617
std::stringstream ss;
617618
ss << "SafetyModeMessage\n";
@@ -642,27 +643,27 @@ TEST_F(PrimaryParserTest, parsing_safetymode_results_in_correct_string)
642643
EXPECT_EQ(data->report_data_type_, static_cast<uint32_t>(i));
643644
if (data->report_data_type_ == 0 || data->report_data_type_ == 1)
644645
{
645-
EXPECT_EQ(data->toString(), construct_string(data->report_data_type_, "2147943941"));
646+
EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "2147943941"));
646647
}
647648
else if (data->report_data_type_ == 2)
648649
{
649-
EXPECT_EQ(data->toString(), construct_string(data->report_data_type_, "-2147023355"));
650+
EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "-2147023355"));
650651
}
651652
else if (data->report_data_type_ == 3)
652653
{
653-
EXPECT_EQ(data->toString(), construct_string(data->report_data_type_, "6.30203e-36"));
654+
EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "6.30203e-36"));
654655
}
655656
else
656657
{
657-
EXPECT_EQ(data->toString(), construct_string(data->report_data_type_, "0x80070605"));
658+
EXPECT_EQ(data->toString(), constructString(data->report_data_type_, "0x80070605"));
658659
}
659660
}
660661
}
661662
}
662663

663664
TEST_F(PrimaryParserTest, parse_configuration_data_without_reserved_fields)
664665
{
665-
std::vector<uint8_t> payload(kConfigurationDataPayloadBytes, 0);
666+
std::vector<uint8_t> payload(CONFIGURATION_DATA_PAYLOAD_BYTES, 0);
666667
std::vector<uint8_t> packet = makeRobotStatePacketWithConfigurationSubmessage(payload);
667668

668669
comm::BinParser bp(packet.data(), packet.size());
@@ -678,7 +679,7 @@ TEST_F(PrimaryParserTest, parse_configuration_data_without_reserved_fields)
678679

679680
TEST_F(PrimaryParserTest, parse_configuration_data_with_reserved_fields)
680681
{
681-
std::vector<uint8_t> payload(kConfigurationDataPayloadBytes, 0);
682+
std::vector<uint8_t> payload(CONFIGURATION_DATA_PAYLOAD_BYTES, 0);
682683
// Big-endian int16 values appended after the legacy payload.
683684
payload.push_back(0x12);
684685
payload.push_back(0x34);
@@ -701,7 +702,7 @@ TEST_F(PrimaryParserTest, parse_configuration_data_with_reserved_fields)
701702
TEST_F(PrimaryParserTest, parse_masterboard_data_without_immi)
702703
{
703704
std::vector<uint8_t> payload;
704-
payload.reserve(kMasterboardPayloadBytesBeforeImmi + kMasterboardPayloadBytesAfterImmi);
705+
payload.reserve(MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI);
705706

706707
appendBigEndian<uint32_t>(payload, 0xDEADBEEFu); // digital input bits
707708
appendBigEndian<uint32_t>(payload, 0xCAFEBABEu); // digital output bits
@@ -725,7 +726,7 @@ TEST_F(PrimaryParserTest, parse_masterboard_data_without_immi)
725726
payload.push_back(0x01); // three position enabling device input (true)
726727
payload.push_back(0x00); // reserved
727728

728-
ASSERT_EQ(payload.size(), kMasterboardPayloadBytesBeforeImmi + kMasterboardPayloadBytesAfterImmi);
729+
ASSERT_EQ(payload.size(), MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI);
729730

730731
std::vector<uint8_t> packet = makeRobotStatePacketWithMasterboardSubmessage(payload);
731732
comm::BinParser bp(packet.data(), packet.size());
@@ -754,13 +755,21 @@ TEST_F(PrimaryParserTest, parse_masterboard_data_without_immi)
754755
EXPECT_FALSE(data->immi_interface_installed_);
755756
EXPECT_EQ(data->operational_mode_selector_input_, 2);
756757
EXPECT_TRUE(data->three_position_enabling_device_input_);
758+
759+
auto msg_str = data->toString();
760+
std::cout << msg_str << std::endl;
761+
EXPECT_NE(msg_str.find("IMMI interface installed: false"), std::string::npos);
762+
EXPECT_EQ(msg_str.find("IMMI input bits:"), std::string::npos);
763+
EXPECT_EQ(msg_str.find("IMMI output bits:"), std::string::npos);
764+
EXPECT_EQ(msg_str.find("IMMI voltage 24V:"), std::string::npos);
765+
EXPECT_EQ(msg_str.find("IMMI current:"), std::string::npos);
757766
}
758767

759768
TEST_F(PrimaryParserTest, parse_masterboard_data_with_immi)
760769
{
761770
std::vector<uint8_t> payload;
762-
payload.reserve(kMasterboardPayloadBytesBeforeImmi + kMasterboardOptionalImmiBytes +
763-
kMasterboardPayloadBytesAfterImmi);
771+
payload.reserve(MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_OPTIONAL_IMMI_BYTES +
772+
MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI);
764773

765774
appendBigEndian<uint32_t>(payload, 0x00000001u);
766775
appendBigEndian<uint32_t>(payload, 0x00000002u);
@@ -788,8 +797,8 @@ TEST_F(PrimaryParserTest, parse_masterboard_data_with_immi)
788797
payload.push_back(0x00);
789798
payload.push_back(0x00);
790799

791-
ASSERT_EQ(payload.size(),
792-
kMasterboardPayloadBytesBeforeImmi + kMasterboardOptionalImmiBytes + kMasterboardPayloadBytesAfterImmi);
800+
ASSERT_EQ(payload.size(), MASTERBOARD_PAYLOAD_BYTES_BEFORE_IMMI + MASTERBOARD_OPTIONAL_IMMI_BYTES +
801+
MASTERBOARD_PAYLOAD_BYTES_AFTER_IMMI);
793802

794803
std::vector<uint8_t> packet = makeRobotStatePacketWithMasterboardSubmessage(payload);
795804
comm::BinParser bp(packet.data(), packet.size());
@@ -806,6 +815,15 @@ TEST_F(PrimaryParserTest, parse_masterboard_data_with_immi)
806815
EXPECT_EQ(data->immi_output_bits_, std::bitset<32>(0x33334444u));
807816
EXPECT_FLOAT_EQ(data->immi_voltage_24v_, 24.0f);
808817
EXPECT_FLOAT_EQ(data->immi_current_, 0.75f);
818+
819+
auto msg_str = data->toString(); // also verify that the presence of IMMI fields does not cause issues in the
820+
// toString() method
821+
std::cout << msg_str << std::endl;
822+
EXPECT_NE(msg_str.find("IMMI interface installed: true"), std::string::npos);
823+
EXPECT_NE(msg_str.find("IMMI input bits: 0b00010001000100010010001000100010"), std::string::npos);
824+
EXPECT_NE(msg_str.find("IMMI output bits: 0b00110011001100110100010001000100"), std::string::npos);
825+
EXPECT_NE(msg_str.find("IMMI voltage 24V: 24"), std::string::npos);
826+
EXPECT_NE(msg_str.find("IMMI current: 0.75"), std::string::npos);
809827
}
810828

811829
int main(int argc, char* argv[])

0 commit comments

Comments
 (0)