41 #include <can_msgs/Frame.h> 43 #include <dbw_fca_msgs/BrakeCmd.h> 44 #include <dbw_fca_msgs/BrakeReport.h> 45 #include <dbw_fca_msgs/ThrottleCmd.h> 46 #include <dbw_fca_msgs/ThrottleReport.h> 47 #include <dbw_fca_msgs/SteeringCmd.h> 48 #include <dbw_fca_msgs/SteeringReport.h> 49 #include <dbw_fca_msgs/GearCmd.h> 50 #include <dbw_fca_msgs/GearReport.h> 51 #include <dbw_fca_msgs/MiscCmd.h> 52 #include <dbw_fca_msgs/Misc1Report.h> 53 #include <dbw_fca_msgs/Misc2Report.h> 54 #include <dbw_fca_msgs/WheelPositionReport.h> 55 #include <dbw_fca_msgs/WheelSpeedReport.h> 56 #include <dbw_fca_msgs/FuelLevelReport.h> 57 #include <dbw_fca_msgs/TirePressureReport.h> 58 #include <dbw_fca_msgs/BrakeInfoReport.h> 59 #include <dbw_fca_msgs/ThrottleInfoReport.h> 60 #include <sensor_msgs/Imu.h> 61 #include <sensor_msgs/NavSatFix.h> 62 #include <sensor_msgs/TimeReference.h> 63 #include <sensor_msgs/JointState.h> 64 #include <geometry_msgs/TwistStamped.h> 65 #include <std_msgs/Empty.h> 66 #include <std_msgs/Bool.h> 67 #include <std_msgs/String.h> 83 void recvEnable(
const std_msgs::Empty::ConstPtr& msg);
84 void recvDisable(
const std_msgs::Empty::ConstPtr& msg);
85 void recvCAN(
const can_msgs::Frame::ConstPtr& msg);
86 void recvBrakeCmd(
const dbw_fca_msgs::BrakeCmd::ConstPtr& msg);
89 void recvGearCmd(
const dbw_fca_msgs::GearCmd::ConstPtr& msg);
91 void recvMiscCmd(
const dbw_fca_msgs::MiscCmd::ConstPtr& msg);
92 void recvCanImu(
const std::vector<can_msgs::Frame::ConstPtr> &msgs);
93 void recvCanGps(
const std::vector<can_msgs::Frame::ConstPtr> &msgs);
116 inline bool fault() {
return fault_brakes_ || fault_throttle_ || fault_steering_ || fault_steering_cal_ ||
fault_watchdog_; }
117 inline bool override() {
return override_brake_ || override_throttle_ || override_steering_ ||
override_gear_; }
118 inline bool clear() {
return enable_ &&
override(); }
151 template <
typename T>
static int sgn(T val) {
152 return ((T)0 < val) - (val < (T)0);
231 #endif // _DBW_NODE_H_
bool fault_watchdog_warned_
void recvCAN(const can_msgs::Frame::ConstPtr &msg)
ros::Publisher pub_wheel_speeds_
ros::Publisher pub_gps_fix_
void overrideThrottle(bool override, bool timeout)
ros::Subscriber sub_turn_signal_
ros::Publisher pub_misc_1_
void faultWatchdog(bool fault, uint8_t src, bool braking)
void recvGearCmd(const dbw_fca_msgs::GearCmd::ConstPtr &msg)
void timeoutThrottle(bool timeout, bool enabled)
ros::Publisher pub_gps_time_
void overrideGear(bool override)
ros::Subscriber sub_steering_
ros::Subscriber sub_throttle_
bool enable_joint_states_
void recvThrottleCmd(const dbw_fca_msgs::ThrottleCmd::ConstPtr &msg)
void recvCanImu(const std::vector< can_msgs::Frame::ConstPtr > &msgs)
void faultSteering(bool fault)
ros::Subscriber sub_brake_
ros::Publisher pub_wheel_positions_
ros::Publisher pub_gps_fix_dr
void recvBrakeCmd(const dbw_fca_msgs::BrakeCmd::ConstPtr &msg)
void faultSteeringCal(bool fault)
void faultThrottle(bool fault)
ros::Publisher pub_brake_info_
void recvSteeringCmd(const dbw_fca_msgs::SteeringCmd::ConstPtr &msg)
ros::Subscriber sub_enable_
void recvDisable(const std_msgs::Empty::ConstPtr &msg)
ros::Publisher pub_throttle_info_
ros::Publisher pub_brake_
std::map< uint8_t, std::string > bdate_
sensor_msgs::JointState joint_state_
bool fault_watchdog_using_brakes_
ros::Publisher pub_joint_states_
void timerCallback(const ros::TimerEvent &event)
dataspeed_can_msg_filters::ApproximateTime sync_gps_
ros::Publisher pub_sys_enable_
void recvEnable(const std_msgs::Empty::ConstPtr &msg)
ros::Publisher pub_throttle_
void timeoutBrake(bool timeout, bool enabled)
void overrideBrake(bool override, bool timeout)
void overrideSteering(bool override, bool timeout)
ros::Subscriber sub_gear_
dataspeed_can_msg_filters::ApproximateTime sync_imu_
void recvCanGps(const std::vector< can_msgs::Frame::ConstPtr > &msgs)
void faultBrakes(bool fault)
ros::Publisher pub_misc_2_
ros::Subscriber sub_disable_
DbwNode(ros::NodeHandle &node, ros::NodeHandle &priv_nh)
ros::Subscriber sub_misc_
void recvTurnSignalCmd(const dbw_fca_msgs::MiscCmd::ConstPtr &msg)
ros::Publisher pub_tire_pressure_
ros::Publisher pub_fuel_level_
void publishJointStates(const ros::Time &stamp, const dbw_fca_msgs::SteeringReport *steering)
void recvMiscCmd(const dbw_fca_msgs::MiscCmd::ConstPtr &msg)
void timeoutSteering(bool timeout, bool enabled)
ros::Publisher pub_twist_
ros::Publisher pub_steering_