34 namespace atlascar_base
68 status_pub_ =
nh_.advertise<atlascar_base::ManagerStatus>(
"status", 1);
123 status_.clutch = status->clutch;
125 status_.steering_wheel = status->steering_wheel;
128 status_.lights_brake=status->lights_brake;
129 status_.lights_reverse=status->lights_reverse;
130 status_.lights_warning=status->lights_warning;
132 status_.emergency=status->emergency;
133 status_.auto_ignition=status->auto_ignition;
134 status_.auto_brake=status->auto_brake;
135 status_.auto_direction=status->auto_direction;
136 status_.auto_clutch=status->auto_clutch;
141 status_.lights_high=status->lights_high;
142 status_.lights_medium=status->lights_medium;
143 status_.lights_left=status->lights_left;
144 status_.lights_right=status->lights_right;
145 status_.lights_danger=status->lights_danger;
147 status_.ignition=status->ignition;
151 status_.throttle_pressure=status->throttle_pressure;
152 status_.brake_pressure=status->brake_pressure;
153 status_.clutch_pressure=status->clutch_pressure;
158 status_.velocity=status->velocity;
160 status_.pulses_sec=status->pulses_sec;
161 status_.revolutions_sec=status->revolutions_sec;
167 status_.gearbox_status=status->status;
172 status_.throttle=status->throttle;
173 status_.throttle_pedal=status->footPedal;
174 status_.auto_throttle=status->mode;
270 cout<<
"Manager::stoppedToMoving::stm_state::default"<<endl;
276 double increment = 0.01;
346 }
else if(
command_->direct_control==2)
372 status_.header.stamp = ros::Time::now();
void loop()
Start main control loop.
atlascar_base::GearboxCommand gearbox_command_
Gearbox command message.
void movingToStopped()
Moving to Stopped.
void setupMessaging()
Start ros message subscribing and advertising.
CarState
Car state machine status.
ros::Publisher command_throttle_pub_
Throttle command publisher.
void toOutgoingMessages(const atlascar_base::ManagerCommandPtr &command)
Messages.
Manager(const ros::NodeHandle &nh)
Class constructor.
ros::Subscriber command_sub_
Ros command subscriber.
atlascar_base::PlcCommand plc_command_
Plc command message.
CarState current_state
Current state of state machine.
~Manager()
Class destructor.
void stateManager(const atlascar_base::ManagerCommandPtr &command)
State Manager.
ros::Publisher command_plc_pub_
Plc command publisher.
atlascar_base::ManagerStatus status_
Status message.
ros::Publisher command_gearbox_pub_
Gearbox command publisher.
ros::Subscriber status_gearbox_sub_
Gearbox status subscriber.
atlascar_base::ThrottleCommand throttle_command_
Throttle command message.
void gearboxStatusCallback(const atlascar_base::GearboxStatusPtr &status)
Gearbox status callback.
TopicQueuePriority< atlascar_base::ManagerCommandPtr > command_queue_
Command queue holding class.
void driverStatusCallback(const human_driver_monitor::HumanDriverMonitorStatusPtr &status)
Driver status callback.
Manager class declaration.
ros::Subscriber status_odometer_sub_
Velocity status subscriber.
void init()
Initialize the class.
void throttleStatusCallback(const atlascar_base::ThrottleStatusPtr &status)
Throttle status callback.
ros::Subscriber status_driver_sub_
Driver status subscriber.
void odometerStatusCallback(const odometer::OdometerStatusPtr &status)
Velocity status callback.
void stoppedToMoving()
Stopped to Moving.
ros::Subscriber status_plc_sub_
Plc status subscriber.
ros::Publisher status_pub_
Ros status publisher.
void plcStatusCallback(const atlascar_base::PlcStatusPtr &status)
Plc status callback.
ros::NodeHandle nh_
Ros node handler.
sequentialState mts_state
Moving to stopped state.
atlascar_base::ManagerCommandPtr safety_command_
Safety command message pointer.
sequentialState stm_state
Stopped to moving state.
atlascar_base::ManagerCommandPtr command_
Command message pointer.
void commandCallback(const atlascar_base::ManagerCommandPtr &command)
Main command callback.
ros::Subscriber status_throttle_sub_
Throttle status subscriber.
CarState getCurrentState()
Check Current State.