forked from DO13-Autonomy/deeporange13-14_control
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathStateMachine.cpp
More file actions
178 lines (152 loc) · 6.91 KB
/
StateMachine.cpp
File metadata and controls
178 lines (152 loc) · 6.91 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
#include <deeporange13_control/StateMachine.h>
namespace deeporange14 {
StateMachine::StateMachine(ros::NodeHandle &node, ros::NodeHandle &priv_nh){
// Instantiate sub/pubs
topic_ns = "/deeporange14"
sub_raptor = node.subscribe(std::string(topic_ns + "/raptor_state"), 10, &StateMachine::getRaptorMsg, this, ros::TransportHints().tcpNoDelay(true));
sub_cmdVel = node.subscribe(std::string(topic_ns +"/cmd_vel"), 10, &StateMachine::getCommandedTwist, this, ros::TransportHints().tcpNoDelay(true));
sub_cmdTrq = node.subscribe(std::string(topic_ns +"/cmd_torq"), 10, &StateMachine::getCommandedTorques, this, ros::TransportHints().tcpNoDelay(true));
pub_trackVel = node.advertise<can_msgs::Frame>("can_tx", 10);
pub_estop = node.advertise<std_msgs::Bool>("fort_estop", 10);
pub_rosState = node.advertise<deeporange14_msgs::RosState>(std::string(topic_ns +"/ros_state"), 10);
// Set up Timer - with calback to publish ROS state all the time that the node is running
timer = node.createTimer(ros::Duration(1.0 / 20.0), &StateMachine::publishROSState, this);
/* Initiate ROS State with a DEFAULT state to be safe. This state will be published till the ...
ROS supervisor intentionally changes it.Default Node is On and it is running continuously in linux service
*/
rosSupMsg.ros_state = AU0_DEFAULT;
rosSupMsg.system_state= SS1_DEFAULT;
rosSupMsg.fault_code = 0;
rosSupMsg.stack_faults = false ;
rosSupMsg.raptorHeartfail = false;
rosSupMsg.counter = 1;
rosSupMsg.brake_enable = true;
rosSupMsg.logging_enable = true;
}
StateMachine::~StateMachine(){}
void StateMachine::publishROSState(const ros::TimerEvent& event)
{
/* Always continue to publish ROS state */
StateMachine::updateROSStateMsg();
pub_rosState.publish(rosSupMsg);
}
void StateMachine::updateROSStateMsg(){
if (rosSupMsg.ros_state == AU1_STARTUP){
ros::Time first_time_counter = ros::Time::now();
if (!isHandshakefailed()){
// we are in Startup Raptor Handshake established
rosSupMsg.ros_state = AU2_IDLE;
}else{
// keep checking for 3 secs
// if still not after 3 sec give Error
while(abs(first_time_counter.toSec())- ros::Time::now().toSec()<= 5){ // allow 5 seconds to {
rosSupMsg.counter = counter++;
if (!isHandshakefailed()){
// we are in Startup Raptor Handshake established
rosSupMsg.ros_state = AU2_IDLE;
}
}
}
}
else if (rosSupMsg.ros_state == AU2_IDLE){
// %set Brakes Enable true % set torques = 0 check ss and ros modecheck isHanshakeFailed()
rosSupMsg.brake_enable = true;
torqueMsg.left_torque = 0;
torqueMsg.right_torque = 0;
if (isHandshakefailed()){
rosSupMsg.ros_state = AU1_STARTUP;
}
if (raptorMsg.system_state == SS8_NOMINALOP && raptorMsg.dbw_mode == 2 ){
rosSupMsg.ros_state = AU3_WAIT_EXECUTION;
}
}
else if (rosSupMsg.ros_state == AU3_WAIT_EXECUTION){
if (isHandshakefailed()){
rosSupMsg.ros_state = AU1_STARTUP;
}
if (isStackFault()){
rosSupMsg.ros_state = AU2_IDLE;
// ****************change dbw mode ***********************
}
rosSupMsg.brake_enable = true;
torqueMsg.left_torque = 0;
torqueMsg.right_torque = 0;
//
// is execution button pressed
// update state if true
//
}
else if (rosSupMsg.ros_state == AU4_EXEC_IMINENT){
if (isHandshakefailed()){
rosSupMsg.ros_state = AU1_STARTUP;
}
if (isStackFault()){
rosSupMsg.ros_state = AU2_IDLE;
// ****************change dbw mode ***********************
}
// // publish brake command and torque commands
rosSupMsg.brake_enable = true;
torqueMsg.left_torque = 0;
torqueMsg.right_torque = 0;
//
// is Global plan ready
// update state if true
//
}
else if (rosSupMsg.ros_state == AU5_DISENGAGE_BRAKE){
if (isHandshakefailed()){
rosSupMsg.ros_state = AU1_STARTUP;
}
if (isStackFault()){
rosSupMsg.ros_state = AU2_IDLE;
// ****************change dbw mode ***********************
}
rosSupMsg.brake_enable = false;
// raptor acknowledged brake disabled
// update state if true
}
else if (rosSupMsg.ros_state == AU6_COMMAND_TORQUES){
if (isHandshakefailed()){
rosSupMsg.ros_state = AU1_STARTUP;
}
if (isStackFault()){
rosSupMsg.ros_state = AU2_IDLE;
// ****************change dbw mode ***********************
}
// is mission completed check from navigation manager
// command status == 6
// Update Ros state
}
else if (rosSupMsg.ros_state == AU98_SAFE_STOP){
if (isHandshakefailed()){
rosSupMsg.ros_state = AU1_STARTUP;
}
if (isStackFault()){
rosSupMsg.ros_state = AU2_IDLE;
// ****************change dbw mode ***********************
}
rosSupMsg.brake_enable = true;
// publish brake command
}
else if (rosSupMsg.ros_state == AU254_HARD_STOP){
// change dbw mode
}
}
bool StateMachine::isHandshakeFailed(){
// checking ros_state shows default values
}
bool StateMachine::isStackFault(){
// Stack Crashed
// Mission Cancelled
// ROS mode/ dbw mode not equal to 3
}
void StateMachine::getCommandedTwist(const geometry_msgs::Twist::ConstPtr& msg){
commandedTwist.linear.x = msg->linear.x;
commandedTwist.angular.z = msg->angular.z;
// Convert to torques
}
void StateMachine::getCommandedTorques(const deeporange14_msgs::TorqueValues::ConstPtr& msg){
torqueMsg.left_torque = msg->left_torque;
torqueMsg.right_torque = msg->right_torque;
}
}