PilotNode Class Reference

controls the ART vehicle brake, throttle, steering and transmission More...

List of all members.

Public Member Functions

 PilotNode (ros::NodeHandle node)
void spin (void)

Private Types

typedef
dynamic_reconfigure::Server
< Config
ReconfigServer

Private Member Functions

void adjustSteering (void)
void halt (void)
void monitorHardware (void)
void processCarCommand (const art_msgs::CarCommand::ConstPtr &msg)
void processCarDrive (const art_msgs::CarDriveStamped::ConstPtr &msg)
void processLearning (const art_msgs::LearningCommand::ConstPtr &learningIn)
void reconfig (Config &newconfig, uint32_t level)
void speedControl (void)
void validateTarget (void)

Private Attributes

boost::shared_ptr
< pilot::AccelBase
accel_
ros::Subscriber accel_cmd_
boost::shared_ptr
< device_interface::DeviceBrake
brake_
ros::Subscriber car_cmd_
Config config_
ros::Time current_time_
ros::Time goal_time_
boost::shared_ptr
< device_interface::DeviceImu
imu_
bool is_shifting_
ros::Subscriber learning_cmd_
boost::shared_ptr
< device_interface::DeviceOdom
odom_
ros::Publisher pilot_state_
art_msgs::PilotState pstate_msg_
boost::shared_ptr< ReconfigServerreconfig_server_
boost::shared_ptr
< device_interface::DeviceShifter
shifter_
boost::shared_ptr
< device_interface::DeviceSteering
steering_
boost::shared_ptr
< device_interface::DeviceThrottle
throttle_
ros::Duration timeout_

Detailed Description

controls the ART vehicle brake, throttle, steering and transmission

The pilot receives CarDriveStamped messages from the navigator, then translates them into commands to the servo motor actuators for controlling the speed and direction of the vehicle. It gets odometry information from a separate node.

Subscribes:

Publishes:

Definition at line 82 of file pilot.cc.


Member Typedef Documentation

typedef dynamic_reconfigure::Server<Config> PilotNode::ReconfigServer [private]

Definition at line 103 of file pilot.cc.


Constructor & Destructor Documentation

PilotNode::PilotNode ( ros::NodeHandle  node  ) 

constructor

Definition at line 142 of file pilot.cc.


Member Function Documentation

void PilotNode::adjustSteering ( void   )  [private]

Adjust steering angle.

We do not use PID control, because the odometry does not provide accurate yaw speed feedback. Instead, we directly compute the corresponding steering angle. We can use open loop control at this level, because navigator monitors our actual course and will request any steering changes needed to reach its goal.

Todo:
Limit angle actually requested based on current velocity to avoid unsafe high speed turns.

Definition at line 213 of file pilot.cc.

void PilotNode::halt ( void   )  [private]

halt -- soft version of hardware E-Stop.

The goal is to bring the vehicle to a halt as quickly as possible, while remaining safely under control. Normally, navigator sends gradually declining speed requests when bringing the vehicle to a controlled stop. The only abrupt requests we see are in "emergency" stop situations, when there was a pause request, or no clear path around an obstacle.

Note:
The target goal velocity may not always be zero, due to the gear shifting requirements.
Postcondition:
Since this function bypasses the normal acceleration controller, it is reset on exit.

Definition at line 245 of file pilot.cc.

void PilotNode::monitorHardware ( void   )  [private]

monitor hardware status based on current inputs

Postcondition:
pstate_msg_ updated to reflect current control hardware status and time of this cycle

Todo:
Optionally check if no commands received recently.

Definition at line 275 of file pilot.cc.

void PilotNode::processCarCommand ( const art_msgs::CarCommand::ConstPtr &  msg  )  [private]

CarCommand message callback (now DEPRECATED)

Definition at line 329 of file pilot.cc.

void PilotNode::processCarDrive ( const art_msgs::CarDriveStamped::ConstPtr &  msg  )  [private]

CarDriveStamped message callback

Definition at line 321 of file pilot.cc.

void PilotNode::processLearning ( const art_msgs::LearningCommand::ConstPtr &  learningIn  )  [private]

LearningCommand message callback (DEPRECATED)

Definition at line 359 of file pilot.cc.

void PilotNode::reconfig ( Config newconfig,
uint32_t  level 
) [private]

handle dynamic reconfigure service request

Parameters:
newconfig new configuration from dynamic reconfigure client, becomes the service reply message as updated here.
level SensorLevels value (0xffffffff on initial call)

This is done without any locking because it is called in the same thread as ros::spinOnce() and all the topic subscription callbacks.

Definition at line 377 of file pilot.cc.

void PilotNode::speedControl ( void   )  [private]

Speed control

Manage the shifter. Inputs are the current and target states. If a gear shift is requested, the vehicle must first be brought to a stop, then one of the transmission shift relays set for one second (then reset), before the vehicle can begin moving in the opposite direction.

Definition at line 405 of file pilot.cc.

void PilotNode::spin ( void   ) 

main loop

Definition at line 177 of file pilot.cc.

void PilotNode::validateTarget ( void   )  [private]

validate target CarDrive values

Definition at line 477 of file pilot.cc.


Member Data Documentation

boost::shared_ptr<pilot::AccelBase> PilotNode::accel_ [private]

Definition at line 133 of file pilot.cc.

ros::Subscriber PilotNode::accel_cmd_ [private]

Definition at line 107 of file pilot.cc.

boost::shared_ptr<device_interface::DeviceBrake> PilotNode::brake_ [private]

Definition at line 111 of file pilot.cc.

ros::Subscriber PilotNode::car_cmd_ [private]

Definition at line 108 of file pilot.cc.

Definition at line 123 of file pilot.cc.

ros::Time PilotNode::current_time_ [private]

Definition at line 126 of file pilot.cc.

ros::Time PilotNode::goal_time_ [private]

Definition at line 129 of file pilot.cc.

boost::shared_ptr<device_interface::DeviceImu> PilotNode::imu_ [private]

Definition at line 112 of file pilot.cc.

bool PilotNode::is_shifting_ [private]

Definition at line 101 of file pilot.cc.

ros::Subscriber PilotNode::learning_cmd_ [private]

Definition at line 118 of file pilot.cc.

boost::shared_ptr<device_interface::DeviceOdom> PilotNode::odom_ [private]

Definition at line 113 of file pilot.cc.

ros::Publisher PilotNode::pilot_state_ [private]

Definition at line 120 of file pilot.cc.

art_msgs::PilotState PilotNode::pstate_msg_ [private]

Definition at line 131 of file pilot.cc.

boost::shared_ptr<ReconfigServer> PilotNode::reconfig_server_ [private]

Definition at line 104 of file pilot.cc.

Definition at line 114 of file pilot.cc.

Definition at line 115 of file pilot.cc.

Definition at line 116 of file pilot.cc.

ros::Duration PilotNode::timeout_ [private]

Definition at line 124 of file pilot.cc.


The documentation for this class was generated from the following file:
 All Classes Namespaces Files Functions Variables Typedefs Friends Defines


art_pilot
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 11 09:11:53 2013