Skip to content

Commit 4528f94

Browse files
cpr-mabcpr-mab
committed
Hardware state info extended
1 parent 08585ef commit 4528f94

8 files changed

Lines changed: 127 additions & 7 deletions

File tree

‎conan_install_linux_all.sh‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
#!/bin/bash
22
# Builds the dependencies and toolchain via conan. This must be called before using CMake.
33

4-
./conan_install_linux_native.sh
4+
./conan_install_linux_x86_64.sh
55
./conan_install_linux_rpi.sh

‎conan_install_linux_rpi.sh‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,4 +31,4 @@ conan install . --lockfile conan_linux_rpi.lock --lockfile-partial --build=missi
3131
conan install . --lockfile conan_linux_rpi.lock --lockfile-partial --build=missing --profile:host=cpr_linux_rpi4_gcc10_debug --profile:build=cpr_linux_x86_64_release -c tools.cmake.cmake_layout:build_folder_vars="['settings.os', 'settings.arch']" -s "&:build_type=Debug" -s "build_type=Release"
3232

3333
echo "Clearing unused dependencies..."
34-
conan remove --lru=12w "*"
34+
conan remove --lru=12w "*" -c

‎conan_install_linux_x86_64.sh‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,4 +31,4 @@ conan install . --lockfile conan_linux_x86_64.lock --lockfile-partial --build=mi
3131
conan install . --lockfile conan_linux_x86_64.lock --lockfile-partial --build=missing --profile=cpr_linux_x86_64_debug -c tools.cmake.cmake_layout:build_folder_vars="['settings.os', 'settings.arch']" -s "&:build_type=Debug" -s "build_type=Release"
3232

3333
echo "Clearing unused dependencies..."
34-
conan remove --lru=12w "*"
34+
conan remove --lru=12w "*" -c

‎conan_install_windows.ps1‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,4 +6,4 @@ conan install . --lockfile conan_windows.lock --lockfile-partial --build=missing
66
#conan install . --lockfile conan_windows.lock --lockfile-partial --build=missing --profile=cpr_windows_x86_64_debug -c tools.cmake.cmake_layout:build_folder_vars="['settings.os', 'settings.arch']" -s:b compiler.cppstd=17
77

88
# Clear unused dependencies
9-
conan remove --lru=12w "*"
9+
conan remove --lru=12w "*" -c

‎protos/robotcontrolapp.proto‎

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -327,6 +327,12 @@ message Joint {
327327
JointPosition position = 3;
328328
// Hardware error state
329329
HardwareState state = 5;
330+
// Hardware state (manufacturer specific code)
331+
uint64 state_code = 11;
332+
// is the robot enabled and ready for motion?
333+
bool is_enabled = 12;
334+
// is the robot in an error state?
335+
bool is_error = 13;
330336

331337
ReferencingState referencing_state = 6;
332338

@@ -359,8 +365,16 @@ message RobotState {
359365
repeated DOut DOuts = 6;
360366
repeated GSig GSigs = 7;
361367

362-
// hardware error state
368+
// hardware state string
363369
string hardware_state_string = 11;
370+
// Hardware state enum
371+
HardwareState hardware_state = 21;
372+
// Hardware state (manufacturer specific code)
373+
uint64 hardware_state_code = 22;
374+
// is the robot enabled and ready for motion?
375+
bool is_enabled = 23;
376+
// is the robot in an error state?
377+
bool is_error = 24;
364378
// kinematic error state
365379
KinematicState kinematic_state = 12;
366380

‎src/DataTypes/RobotState.cpp‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,10 @@ RobotState::RobotState(const robotcontrolapp::RobotState& state)
5252
}
5353

5454
hardwareState = state.hardware_state_string();
55+
hardwareStateEnum = static_cast<DataTypes::RobotState::HardwareState>(state.hardware_state());
56+
hardwareStateCode = state.hardware_state_code();
57+
isEnabled = state.is_enabled();
58+
isError = state.is_error();
5559
kinematicState = state.kinematic_state();
5660
velocityOverride = state.velocity_override();
5761
cartesianVelocity = state.cartesian_velocity();
@@ -86,6 +90,9 @@ RobotState::Joint::Joint(const robotcontrolapp::Joint& joint)
8690
targetPosition = joint.position().target_position();
8791

8892
hardwareState = static_cast<DataTypes::RobotState::HardwareState>(joint.state());
93+
hardwareStateCode = joint.state_code();
94+
isEnabled = joint.is_enabled();
95+
isError = joint.is_error();
8996

9097
switch (joint.referencing_state())
9198
{
@@ -114,7 +121,7 @@ RobotState::Joint::Joint(const robotcontrolapp::Joint& joint)
114121
*/
115122
bool RobotState::IsEnabled() const
116123
{
117-
return hardwareState == "NoError";
124+
return isEnabled;
118125
}
119126

120127
} // namespace DataTypes

‎src/DataTypes/RobotState.h‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,19 @@ class RobotState
9191
*/
9292
HardwareState hardwareState = HardwareState::OKAY;
9393

94+
/**
95+
* @brief Manufacturer specific hardware state code
96+
*/
97+
uint64_t hardwareStateCode = 0x0;
98+
/**
99+
* @brief Is the motor enabled and ready to move?
100+
*/
101+
bool isEnabled = false;
102+
/**
103+
* @brief Is the joint in error state?
104+
*/
105+
bool isError = false;
106+
94107
/**
95108
* @brief Referencing state
96109
*/
@@ -148,6 +161,24 @@ class RobotState
148161
* @brief A string describing the combined state of all modules
149162
*/
150163
std::string hardwareState;
164+
165+
/**
166+
* @brief Hardware state enum value
167+
*/
168+
HardwareState hardwareStateEnum = HardwareState::OKAY;
169+
/**
170+
* @brief Manufacturer specific hardware state code
171+
*/
172+
uint64_t hardwareStateCode = 0;
173+
/**
174+
* @brief Are the motors enabled and ready to move?
175+
*/
176+
bool isEnabled = false;
177+
/**
178+
* @brief Is any module in error state?
179+
*/
180+
bool isError = false;
181+
151182
/**
152183
* @brief Kinematic state / error
153184
*/

‎tests/DataTypes/RobotStateTest.cpp‎

Lines changed: 69 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,9 @@ TEST(App_RobotStateJointTest, ConstructorDefault)
1313
EXPECT_DOUBLE_EQ(0, joint.targetPosition);
1414
EXPECT_EQ(App::DataTypes::RobotState::HardwareState::OKAY, joint.hardwareState);
1515
EXPECT_EQ(App::DataTypes::RobotState::ReferencingState::NOT_REFERENCED, joint.referencingState);
16+
EXPECT_EQ(0, joint.hardwareStateCode);
17+
EXPECT_FALSE(joint.isEnabled);
18+
EXPECT_FALSE(joint.isError);
1619
EXPECT_FLOAT_EQ(0, joint.temperatureBoard);
1720
EXPECT_FLOAT_EQ(0, joint.temperatureMotor);
1821
EXPECT_FLOAT_EQ(0, joint.current);
@@ -28,6 +31,9 @@ TEST(App_RobotStateJointTest, ConstructorGRPC)
2831
jointGrpc.mutable_position()->set_position(123.4);
2932
jointGrpc.mutable_position()->set_target_position(234.5);
3033
jointGrpc.set_state(robotcontrolapp::HardwareState::ERROR_ESTOP_LOW_VOLTAGE);
34+
jointGrpc.set_state_code(1234);
35+
jointGrpc.set_is_enabled(true);
36+
jointGrpc.set_is_error(false);
3137
jointGrpc.set_referencing_state(robotcontrolapp::ReferencingState::IS_REFERENCING);
3238
jointGrpc.set_temperature_board(45.6f);
3339
jointGrpc.set_temperature_motor(56.7f);
@@ -41,6 +47,9 @@ TEST(App_RobotStateJointTest, ConstructorGRPC)
4147
EXPECT_DOUBLE_EQ(123.4, joint.actualPosition);
4248
EXPECT_DOUBLE_EQ(234.5, joint.targetPosition);
4349
EXPECT_EQ(App::DataTypes::RobotState::HardwareState::ERROR_ESTOP_LOW_VOLTAGE, joint.hardwareState);
50+
EXPECT_EQ(1234, joint.hardwareStateCode);
51+
EXPECT_TRUE(joint.isEnabled);
52+
EXPECT_FALSE(joint.isError);
4453
EXPECT_EQ(App::DataTypes::RobotState::ReferencingState::IS_REFERENCING, joint.referencingState);
4554
EXPECT_FLOAT_EQ(45.6f, joint.temperatureBoard);
4655
EXPECT_FLOAT_EQ(56.7f, joint.temperatureMotor);
@@ -54,13 +63,17 @@ TEST(App_RobotStateJointTest, ConstructorGRPC)
5463
(robotcontrolapp::HardwareState)(robotcontrolapp::HardwareState::ERROR_MOTOR_NOT_ENABLED | robotcontrolapp::HardwareState::ERROR_DRIVER);
5564
jointGrpc.set_state(state);
5665
jointGrpc.set_referencing_state(robotcontrolapp::ReferencingState::NOT_REFERENCED);
66+
jointGrpc.set_is_enabled(false);
67+
jointGrpc.set_is_error(true);
5768

5869
App::DataTypes::RobotState::Joint joint(jointGrpc);
5970

6071
App::DataTypes::RobotState::HardwareState expectState =
6172
(App::DataTypes::RobotState::HardwareState)((int)App::DataTypes::RobotState::HardwareState::ERROR_MOTOR_NOT_ENABLED |
6273
(int)App::DataTypes::RobotState::HardwareState::ERROR_DRIVER);
6374
EXPECT_EQ(expectState, joint.hardwareState);
75+
EXPECT_FALSE(joint.isEnabled);
76+
EXPECT_TRUE(joint.isError);
6477
EXPECT_EQ(App::DataTypes::RobotState::ReferencingState::NOT_REFERENCED, joint.referencingState);
6578
}
6679

@@ -116,6 +129,11 @@ TEST(App_RobotStateTest, ConstructorDefault)
116129
}
117130

118131
EXPECT_TRUE(robotState.hardwareState.empty());
132+
EXPECT_EQ(App::DataTypes::RobotState::HardwareState::OKAY, robotState.hardwareStateEnum);
133+
EXPECT_EQ(0, robotState.hardwareStateCode);
134+
EXPECT_FALSE(robotState.isEnabled);
135+
EXPECT_FALSE(robotState.isError);
136+
119137
EXPECT_EQ(robotcontrolapp::KinematicState::KINEMATIC_NORMAL, robotState.kinematicState);
120138
EXPECT_FLOAT_EQ(0, robotState.velocityOverride);
121139
EXPECT_FLOAT_EQ(0, robotState.cartesianVelocity);
@@ -159,6 +177,11 @@ TEST(App_RobotStateTest, ConstructorGRPC)
159177
}
160178

161179
robotStateGrpc.set_hardware_state_string("MNE_ENC");
180+
robotStateGrpc.set_hardware_state(robotcontrolapp::HardwareState::ERROR_ESTOP_LOW_VOLTAGE);
181+
robotStateGrpc.set_hardware_state_code(2345);
182+
robotStateGrpc.set_is_enabled(true);
183+
robotStateGrpc.set_is_error(false);
184+
162185
robotStateGrpc.set_kinematic_state(robotcontrolapp::KinematicState::KINEMATIC_ERROR_BRAKE_ACTIVE);
163186
robotStateGrpc.set_velocity_override(0.8f);
164187
robotStateGrpc.set_cartesian_velocity(45.6f);
@@ -206,6 +229,11 @@ TEST(App_RobotStateTest, ConstructorGRPC)
206229
}
207230

208231
EXPECT_STREQ("MNE_ENC", robotState.hardwareState.c_str());
232+
EXPECT_EQ(App::DataTypes::RobotState::HardwareState::ERROR_ESTOP_LOW_VOLTAGE, robotState.hardwareStateEnum);
233+
EXPECT_EQ(2345, robotState.hardwareStateCode);
234+
EXPECT_TRUE(robotState.isEnabled);
235+
EXPECT_FALSE(robotState.isError);
236+
209237
EXPECT_EQ(robotcontrolapp::KinematicState::KINEMATIC_ERROR_BRAKE_ACTIVE, robotState.kinematicState);
210238
EXPECT_FLOAT_EQ(0.8f, robotState.velocityOverride);
211239
EXPECT_FLOAT_EQ(45.6f, robotState.cartesianVelocity);
@@ -320,7 +348,47 @@ TEST(App_RobotStateTest, ConstructorGRPC)
320348
{
321349
EXPECT_EQ(i % 2 == 0, robotState.globalSignals[i]);
322350
}
351+
}
352+
353+
// isEnabled and isError swapped
354+
{
355+
robotcontrolapp::RobotState robotStateGrpc;
356+
robotStateGrpc.set_is_enabled(false);
357+
robotStateGrpc.set_is_error(true);
358+
359+
App::DataTypes::Matrix44 tcp;
360+
tcp.SetOrientation(10, 20, 30);
361+
tcp.Translate(100, 200, 300);
362+
*robotStateGrpc.mutable_tcp() = tcp.ToGrpc();
323363

324-
EXPECT_EQ(App::DataTypes::RobotState::ReferencingState::IS_REFERENCED, robotState.referencingState);
364+
// Test
365+
App::DataTypes::RobotState robotState(robotStateGrpc);
366+
367+
EXPECT_FALSE(robotState.isEnabled);
368+
EXPECT_TRUE(robotState.isError);
325369
}
326370
}
371+
372+
TEST(App_RobotStateTest, IsEnabled)
373+
{
374+
App::DataTypes::RobotState rs;
375+
EXPECT_FALSE(rs.IsEnabled());
376+
377+
rs.hardwareState = "NoError";
378+
rs.hardwareStateEnum = App::DataTypes::RobotState::HardwareState::OKAY;
379+
rs.hardwareStateCode = 0x0;
380+
rs.isEnabled = false;
381+
rs.isError = false;
382+
EXPECT_FALSE(rs.IsEnabled());
383+
384+
rs.isEnabled = true;
385+
EXPECT_TRUE(rs.IsEnabled());
386+
387+
rs.hardwareState = "MNE_COM";
388+
rs.hardwareStateEnum = App::DataTypes::RobotState::HardwareState::ERROR_ENCODER;
389+
rs.hardwareStateCode = 0xFF;
390+
EXPECT_TRUE(rs.IsEnabled());
391+
392+
rs.isError = true;
393+
EXPECT_TRUE(rs.IsEnabled());
394+
}

0 commit comments

Comments
 (0)