Skip to content

Commit 62e6285

Browse files
danhennagedanhennage
authored
Merge pull request #21 from AutoModality/PL-588/add_active_trans
Pl 588/add active trans
2 parents 649258a + 3ca27cb commit 62e6285

File tree

10 files changed

+899
-198
lines changed

10 files changed

+899
-198
lines changed

‎include/am_super/super_state.h‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,16 +5,21 @@
55

66
enum class SuperState : std::uint8_t
77
{
8-
UNKNOWN = brain_box_msgs::VxState::UNKNOWN,
8+
OFF = brain_box_msgs::VxState::OFF,
99
BOOTING = brain_box_msgs::VxState::BOOTING,
1010
READY = brain_box_msgs::VxState::READY,
11+
ARMING = brain_box_msgs::VxState::ARMING,
1112
ARMED = brain_box_msgs::VxState::ARMED,
12-
AUTO_FEATURE = brain_box_msgs::VxState::AUTO_FEATURE,
13-
AUTO_NO_FEATURE = brain_box_msgs::VxState::AUTO_NO_FEATURE,
13+
AUTO = brain_box_msgs::VxState::AUTO,
14+
SEMI_AUTO = brain_box_msgs::VxState::SEMI_AUTO,
1415
HOLD = brain_box_msgs::VxState::HOLD,
15-
ERROR = brain_box_msgs::VxState::ERROR,
16+
ABORT = brain_box_msgs::VxState::ABORT,
1617
MANUAL = brain_box_msgs::VxState::MANUAL,
18+
SHUTDOWN = brain_box_msgs::VxState::SHUTDOWN,
1719
LAST_STATE = brain_box_msgs::VxState::LAST_STATE
1820
};
1921

2022
#endif
23+
24+
// BOOTING_WAIT_ONLINE_OK = brain_box_msgs::VxState::BOOTING,
25+
// BOOTING_WAIT_CONFIGURE_OK = brain_box_msgs::VxState::BOOTING_WAIT_ACTIVE_OK,

‎include/super_lib/am_life_cycle.h‎

Lines changed: 68 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -15,14 +15,25 @@ namespace am
1515
class AMLifeCycle
1616
{
1717
private:
18+
static constexpr double DEFAULT_OK_THROTTLE_S = 10.0;
19+
static constexpr double DEFAULT_WARN_THROTTLE_S = 2.0;
20+
static constexpr double DEFAULT_ERROR_THROTTLE_S = 1.0;
21+
1822
LifeCycleState state_;
1923
LifeCycleStatus status_;
24+
double ok_throttle_s_ = DEFAULT_OK_THROTTLE_S;
25+
double warn_throttle_s_ = DEFAULT_WARN_THROTTLE_S;
26+
double error_throttle_s_ = DEFAULT_ERROR_THROTTLE_S;
2027

21-
public:
22-
LifeCycleState getState() const;
2328
void setState(const LifeCycleState state);
24-
LifeCycleStatus getStatus() const;
25-
void setStatus(const LifeCycleStatus status);
29+
30+
void transition(std::string transition_name, LifeCycleState initial_state, LifeCycleState transition_state,
31+
LifeCycleState new_state, std::function<void(void)> on_function);
32+
void doTransition(std::string transition_name, bool success, LifeCycleState success_state,
33+
LifeCycleState failure_state);
34+
35+
public:
36+
static constexpr std::string_view BROADCAST_NODE_NAME = "";
2637

2738
static constexpr std::string_view STATE_INVALID_STRING = "INVALID";
2839
static constexpr std::string_view STATE_UNCONFIGURED_STRING = "UNCONFIGURED";
@@ -39,17 +50,33 @@ class AMLifeCycle
3950
static constexpr std::string_view STATUS_WARN_STRING = "WARN";
4051
static constexpr std::string_view STATUS_ERROR_STRING = "ERROR";
4152

53+
static constexpr std::string_view COMMAND_CREATE_STRING = "CREATE";
54+
static constexpr std::string_view COMMAND_CONFIGURE_STRING = "CONFIGURE";
55+
static constexpr std::string_view COMMAND_CLEANUP_STRING = "CLEANUP";
56+
static constexpr std::string_view COMMAND_ACTIVATE_STRING = "ACTIVATE";
57+
static constexpr std::string_view COMMAND_DEACTIVATE_STRING = "DEACTIVATE";
58+
static constexpr std::string_view COMMAND_SHUTDOWN_STRING = "SHUTDOWN";
59+
static constexpr std::string_view COMMAND_DESTROY_STRING = "DESTROY";
60+
61+
static constexpr std::string_view EMPTY_STRING = "";
62+
4263
static const std::string_view& stateToString(LifeCycleState state);
64+
static bool stringToState(std::string& state_str, LifeCycleState& state);
4365
static const std::string_view& statusToString(LifeCycleStatus status);
66+
static bool stringToStatus(std::string& status_str, LifeCycleStatus& status);
67+
static const std::string_view& commandToString(LifeCycleCommand command);
68+
static bool stringToCommand(std::string& status_str, LifeCycleCommand& command);
4469

4570
protected:
71+
std::string node_name_;
72+
4673
diagnostic_updater::Updater updater_;
4774
AMStatList stats_list_;
4875

4976
ros::NodeHandle nh_;
5077
ros::Timer heartbeat_timer_;
5178
ros::Publisher state_pub_;
52-
79+
ros::Subscriber lifecycle_sub_;
5380
/**
5481
* @brief Default constructor
5582
*/
@@ -60,51 +87,72 @@ class AMLifeCycle
6087
*/
6188
virtual ~AMLifeCycle();
6289

63-
virtual void configure();
64-
virtual void cleanup();
65-
virtual void activate();
66-
virtual void deactivate();
67-
virtual void shutdown();
68-
virtual void destroy();
69-
7090
/**
7191
* @brief Function to be defined by the user.
72-
* Called at the end of transition from UNCONFIGURED to INACTIVE.
92+
* Called at the end of transition from INACTIVE to ACTIVE.
7393
*/
74-
virtual void onConfigure();
94+
void activate();
95+
virtual void onActivate();
96+
void doActivate(bool success);
7597

7698
/**
7799
* @brief Function to be defined by the user.
78100
* Called at the end of transition from INACTIVE to UNCONFIGURED.
79101
*/
80-
virtual void onCleanUp();
102+
void cleanup();
103+
virtual void onCleanup();
104+
void doCleanup(bool success);
81105

82106
/**
83107
* @brief Function to be defined by the user.
84-
* Called at the end of transition from INACTIVE to ACTIVE.
108+
* Called at the end of transition from UNCONFIGURED to INACTIVE.
85109
*/
86-
virtual void onActivate();
110+
void configure();
111+
virtual void onConfigure();
112+
void doConfigure(bool success);
87113

88114
/**
89115
* @brief Function to be defined by the user.
90116
* Called at the end of transition from ACTIVE to INACTIVE.
91117
*/
118+
void deactivate();
92119
virtual void onDeactivate();
120+
void doDeactivate(bool success);
93121

94122
/**
95123
* @brief Function to be defined by the user.
96-
* Called at the end of transition from INACTIVE to FINALIZED.
124+
* Called at the end of transition from FINALIZED to power off.
97125
*/
98-
virtual void onShutdown();
126+
virtual void destroy();
127+
virtual void onDestroy();
128+
void doDestroy(bool success);
99129

100130
/**
101131
* @brief Function to be defined by the user.
102-
* Called after an error and may transition to UNCONFIGURED or FINALIZED.
132+
* Called at any time and transitions to UNCONFIGURED or FINALIZED.
103133
*/
134+
void error();
104135
virtual void onError();
136+
void doError(bool success);
137+
138+
/**
139+
* @brief Function to be defined by the user.
140+
* Called at the end of transition from INACTIVE to FINALIZED.
141+
*/
142+
void shutdown();
143+
virtual void onShutdown();
144+
void doShutdown(bool success);
105145

106146
virtual void addStatistics(diagnostic_updater::DiagnosticStatusWrapper& dsw);
107147
virtual void heartbeatCB(const ros::TimerEvent& event);
148+
void sendNodeUpdate();
149+
void lifecycleCB(const brain_box_msgs::LifeCycleCommand::ConstPtr msg);
150+
151+
LifeCycleState getState() const;
152+
LifeCycleStatus getStatus() const;
153+
void setStatus(const LifeCycleStatus status);
154+
double getThrottleS() const;
155+
void setThrottleS(const double throttleS);
108156

109157
}; // class AMLifeCycle
110158

‎include/super_lib/am_life_cycle_types.h‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,9 +3,13 @@
33

44
#include <cstdint>
55
#include <brain_box_msgs/LifeCycleState.h>
6+
#include <brain_box_msgs/LifeCycleCommand.h>
67

78
namespace am
89
{
10+
/**
11+
* state is the lifecycle state which is about startup, shutdown, and error handling
12+
*/
913
enum class LifeCycleState : std::uint8_t
1014
{
1115
INVALID = brain_box_msgs::LifeCycleState::STATE_INVALID,
@@ -22,12 +26,31 @@ enum class LifeCycleState : std::uint8_t
2226
LAST_STATE = brain_box_msgs::LifeCycleState::STATE_LAST
2327
};
2428

29+
/**
30+
* status of the functionality of the node (i.e. is it operating to spec)
31+
*/
2532
enum class LifeCycleStatus : std::uint8_t
2633
{
2734
OK = brain_box_msgs::LifeCycleState::STATUS_OK,
2835
WARN = brain_box_msgs::LifeCycleState::STATUS_WARN,
2936
ERROR = brain_box_msgs::LifeCycleState::STATUS_ERROR,
3037
LAST_STATUS = brain_box_msgs::LifeCycleState::STATUS_LAST
3138
};
39+
40+
/**
41+
* lifecycle commands to nodes to change state
42+
*/
43+
enum class LifeCycleCommand : std::uint8_t
44+
{
45+
CREATE = brain_box_msgs::LifeCycleCommand::COMMAND_CREATE,
46+
CONFIGURE = brain_box_msgs::LifeCycleCommand::COMMAND_CONFIGURE,
47+
CLEANUP = brain_box_msgs::LifeCycleCommand::COMMAND_CLEANUP,
48+
ACTIVATE = brain_box_msgs::LifeCycleCommand::COMMAND_ACTIVATE,
49+
DEACTIVATE = brain_box_msgs::LifeCycleCommand::COMMAND_DEACTIVATE,
50+
SHUTDOWN = brain_box_msgs::LifeCycleCommand::COMMAND_SHUTDOWN,
51+
DESTROY = brain_box_msgs::LifeCycleCommand::COMMAND_DESTROY,
52+
LAST_COMMAND = brain_box_msgs::LifeCycleCommand::COMMAND_LAST
3253
};
54+
55+
}; // namespace am
3356
#endif

‎include/super_lib/am_stat.h‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,8 @@ class AMStat
3131
}
3232

3333
AMStat(const std::string& short_name, const std::string& long_name, uint32_t max_warn, uint32_t max_error)
34+
: AMStat(short_name, long_name)
3435
{
35-
AMStat(short_name, long_name);
3636
max_warn_ = max_warn;
3737
max_error_ = max_error;
3838
}
@@ -41,18 +41,20 @@ class AMStat
4141
{
4242
}
4343

44-
virtual LifeCycleStatus process()
44+
virtual LifeCycleStatus process(double warn_throttle_s, double error_throttle_s)
4545
{
4646
LifeCycleStatus status = LifeCycleStatus::OK;
4747

4848
if (value_ > max_error_)
4949
{
50-
ROS_ERROR_STREAM_THROTTLE(1.0, long_name_ << " exceeding max_error: " << value_ << " (max:" << max_error_ << ")");
50+
ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " exceeding max_error: " << value_
51+
<< " (max:" << max_error_ << ")");
5152
compoundStatus(status, LifeCycleStatus::ERROR);
5253
}
5354
else if (value_ > max_warn_)
5455
{
55-
ROS_WARN_STREAM_THROTTLE(1.0, long_name_ << " exceeding max_warn: " << value_ << " (max:" << max_warn_ << ")");
56+
ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " exceeding max_warn: " << value_ << " (max:" << max_warn_
57+
<< ")");
5658
compoundStatus(status, LifeCycleStatus::WARN);
5759
}
5860

‎include/super_lib/am_stat_ave.h‎

Lines changed: 17 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -47,58 +47,60 @@ class AMStatAve : public AMStatReset
4747
max_max_error_ = max_max_error;
4848
}
4949

50-
LifeCycleStatus process() override
50+
LifeCycleStatus process(double warn_throttle_s, double error_throttle_s) override
5151
{
5252
uint32_t ave = getAve();
5353
LifeCycleStatus status = LifeCycleStatus::OK;
5454

5555
if (ave > max_error_)
5656
{
57-
ROS_ERROR_STREAM_THROTTLE(1.0, long_name_ << " ave exceeding max_error: " << ave << " (max:" << max_error_
58-
<< ")");
57+
ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " ave exceeding max_error: " << ave
58+
<< " (max:" << max_error_ << ")");
5959
compoundStatus(status, LifeCycleStatus::ERROR);
6060
}
6161
else if (ave > max_warn_)
6262
{
63-
ROS_WARN_STREAM_THROTTLE(1.0, long_name_ << " ave exceeding max_warn: " << ave << " (max:" << max_warn_ << ")");
63+
ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " ave exceeding max_warn: " << ave
64+
<< " (max:" << max_warn_ << ")");
6465
compoundStatus(status, LifeCycleStatus::WARN);
6566
}
6667

6768
if (ave < min_error_)
6869
{
69-
ROS_ERROR_STREAM_THROTTLE(1.0, long_name_ << " ave exceeding min_error: " << ave << " (min:" << min_error_
70-
<< ")");
70+
ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " ave exceeding min_error: " << ave
71+
<< " (min:" << min_error_ << ")");
7172
compoundStatus(status, LifeCycleStatus::ERROR);
7273
}
7374
else if (ave < min_warn_)
7475
{
75-
ROS_WARN_STREAM_THROTTLE(1.0, long_name_ << " ave exceeding min_warn: " << ave << " (min:" << min_warn_ << ")");
76+
ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " ave exceeding min_warn: " << ave
77+
<< " (min:" << min_warn_ << ")");
7678
compoundStatus(status, LifeCycleStatus::WARN);
7779
}
7880

7981
if (min_ < min_min_error_)
8082
{
81-
ROS_ERROR_STREAM_THROTTLE(1.0, long_name_ << " min exceeding min_min_error: " << min_
82-
<< " (min:" << min_min_error_ << ")");
83+
ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " min exceeding min_min_error: " << min_
84+
<< " (min:" << min_min_error_ << ")");
8385
compoundStatus(status, LifeCycleStatus::ERROR);
8486
}
8587
else if (min_ < min_min_warn_)
8688
{
87-
ROS_WARN_STREAM_THROTTLE(1.0, long_name_ << " min exceeding min_min_warn: " << min_ << " (min:" << min_min_warn_
88-
<< ")");
89+
ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " min exceeding min_min_warn: " << min_
90+
<< " (min:" << min_min_warn_ << ")");
8991
compoundStatus(status, LifeCycleStatus::WARN);
9092
}
9193

9294
if (max_ > max_max_error_)
9395
{
94-
ROS_ERROR_STREAM_THROTTLE(1.0, long_name_ << " max exceeding max_max_error: " << max_
95-
<< " (max:" << max_max_error_ << ")");
96+
ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " max exceeding max_max_error: " << max_
97+
<< " (max:" << max_max_error_ << ")");
9698
compoundStatus(status, LifeCycleStatus::ERROR);
9799
}
98100
else if (max_ > max_max_warn_)
99101
{
100-
ROS_WARN_STREAM_THROTTLE(1.0, long_name_ << " max exceeding max_max_warn: " << max_ << " (max:" << max_max_warn_
101-
<< ")");
102+
ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " max exceeding max_max_warn: " << max_
103+
<< " (max:" << max_max_warn_ << ")");
102104
compoundStatus(status, LifeCycleStatus::WARN);
103105
}
104106

‎include/super_lib/am_stat_list.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,13 +23,13 @@ class AMStatList
2323
stat_list_.push_back(stat);
2424
}
2525

26-
LifeCycleStatus process()
26+
LifeCycleStatus process(double warn_throttle_s, double error_throttle_s)
2727
{
2828
LifeCycleStatus status = LifeCycleStatus::OK;
2929

3030
for (AMStat* stat : stat_list_)
3131
{
32-
AMStat::compoundStatus(status, stat->process());
32+
AMStat::compoundStatus(status, stat->process(warn_throttle_s, error_throttle_s));
3333
}
3434

3535
return status;

‎include/super_lib/am_stat_reset.h‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,18 +34,20 @@ class AMStatReset : public AMStat
3434
max_error_ = max_error;
3535
}
3636

37-
LifeCycleStatus process() override
37+
LifeCycleStatus process(double warn_throttle_s, double error_throttle_s) override
3838
{
39-
LifeCycleStatus status = AMStat::process();
39+
LifeCycleStatus status = AMStat::process(warn_throttle_s, error_throttle_s);
4040

4141
if (value_ < min_error_)
4242
{
43-
ROS_ERROR_STREAM_THROTTLE(1.0, long_name_ << " exceeding min_error: " << value_ << " (min:" << min_error_ << ")");
43+
ROS_ERROR_STREAM_THROTTLE(error_throttle_s, long_name_ << " exceeding min_error: " << value_
44+
<< " (min:" << min_error_ << ")");
4445
compoundStatus(status, LifeCycleStatus::ERROR);
4546
}
4647
else if (value_ < min_warn_)
4748
{
48-
ROS_WARN_STREAM_THROTTLE(1.0, long_name_ << " exceeding min_warn: " << value_ << " (min:" << min_warn_ << ")");
49+
ROS_WARN_STREAM_THROTTLE(warn_throttle_s, long_name_ << " exceeding min_warn: " << value_ << " (min:" << min_warn_
50+
<< ")");
4951
compoundStatus(status, LifeCycleStatus::WARN);
5052
}
5153

‎launch/am_super.yaml‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +0,0 @@
1-
status_log_level: 1
2-
timeout_s: 10.0
3-
state_machine_duration: 0.1
4-
manifest: ""

0 commit comments

Comments
 (0)