dbw_pacifica_can: DbwNode.h Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2018-2019 New Eagle 
00005  *  Copyright (c) 2015-2018, Dataspeed Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Dataspeed Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #ifndef _DBW_NODE_H_
00037 #define _DBW_NODE_H_
00038 
00039 #include <ros/ros.h>
00040 
00041 // ROS messages
00042 #include <can_msgs/Frame.h>
00043 #include <dbw_pacifica_msgs/BrakeCmd.h>
00044 #include <dbw_pacifica_msgs/BrakeReport.h>
00045 #include <dbw_pacifica_msgs/AcceleratorPedalCmd.h>
00046 #include <dbw_pacifica_msgs/AcceleratorPedalReport.h>
00047 #include <dbw_pacifica_msgs/SteeringCmd.h>
00048 #include <dbw_pacifica_msgs/SteeringReport.h>
00049 #include <dbw_pacifica_msgs/GearCmd.h>
00050 #include <dbw_pacifica_msgs/GearReport.h>
00051 #include <dbw_pacifica_msgs/MiscCmd.h>
00052 #include <dbw_pacifica_msgs/MiscReport.h>
00053 #include <dbw_pacifica_msgs/WheelPositionReport.h>
00054 #include <dbw_pacifica_msgs/WheelSpeedReport.h>
00055 #include <dbw_pacifica_msgs/TirePressureReport.h>
00056 #include <dbw_pacifica_msgs/SurroundReport.h>
00057 #include <dbw_pacifica_msgs/DriverInputReport.h>
00058 #include <dbw_pacifica_msgs/LowVoltageSystemReport.h>
00059 #include <dbw_pacifica_msgs/ActuatorControlMode.h>
00060 #include <dbw_pacifica_msgs/Brake2Report.h>
00061 #include <dbw_pacifica_msgs/Steering2Report.h>
00062 #include <dbw_pacifica_msgs/GlobalEnableCmd.h>
00063 #include <dbw_pacifica_msgs/FaultActionsReport.h>
00064 #include <dbw_pacifica_msgs/HmiGlobalEnableReport.h>
00065 
00066 #include <sensor_msgs/Imu.h>
00067 #include <sensor_msgs/JointState.h>
00068 #include <geometry_msgs/TwistStamped.h>
00069 #include <std_msgs/Empty.h>
00070 #include <std_msgs/Bool.h>
00071 #include <std_msgs/String.h>
00072 
00073 //#include <dbc/DbcUtilities.h>
00074 #include <dbc/DbcMessage.h>
00075 #include <dbc/DbcSignal.h>
00076 #include <dbc/Dbc.h>
00077 #include <dbc/DbcBuilder.h>
00078 
00079 #include <pdu_msgs/RelayCommand.h>
00080 #include <pdu_msgs/RelayState.h>
00081 
00082 namespace dbw_pacifica_can
00083 {
00084 
00085 class DbwNode
00086 {
00087 public:
00088   DbwNode(ros::NodeHandle &node, ros::NodeHandle &priv_nh);
00089   ~DbwNode();
00090 
00091 private:
00092   void timerCallback(const ros::TimerEvent& event);
00093   void recvEnable(const std_msgs::Empty::ConstPtr& msg);
00094   void recvDisable(const std_msgs::Empty::ConstPtr& msg);
00095   void recvCAN(const can_msgs::Frame::ConstPtr& msg);
00096   void recvCanImu(const std::vector<can_msgs::Frame::ConstPtr> &msgs);
00097   void recvCanGps(const std::vector<can_msgs::Frame::ConstPtr> &msgs);
00098   void recvBrakeCmd(const dbw_pacifica_msgs::BrakeCmd::ConstPtr& msg);
00099   void recvAcceleratorPedalCmd(const dbw_pacifica_msgs::AcceleratorPedalCmd::ConstPtr& msg);
00100   void recvSteeringCmd(const dbw_pacifica_msgs::SteeringCmd::ConstPtr& msg);
00101   void recvGearCmd(const dbw_pacifica_msgs::GearCmd::ConstPtr& msg);
00102   void recvMiscCmd(const dbw_pacifica_msgs::MiscCmd::ConstPtr& msg);
00103   void recvGlobalEnableCmd(const dbw_pacifica_msgs::GlobalEnableCmd::ConstPtr& msg);
00104 
00105   ros::Timer timer_;
00106   bool prev_enable_;
00107   bool enable_;
00108   bool override_brake_;
00109   bool override_accelerator_pedal_;
00110   bool override_steering_;
00111   bool override_gear_;
00112   bool fault_brakes_;
00113   bool fault_accelerator_pedal_;
00114   bool fault_steering_;
00115   bool fault_steering_cal_;
00116   bool fault_watchdog_;
00117   bool fault_watchdog_using_brakes_;
00118   bool fault_watchdog_warned_;
00119   bool timeout_brakes_;
00120   bool timeout_accelerator_pedal_;
00121   bool timeout_steering_;
00122   bool enabled_brakes_;
00123   bool enabled_accelerator_pedal_;
00124   bool enabled_steering_;
00125   bool gear_warned_;
00126   inline bool fault() { return fault_brakes_ || fault_accelerator_pedal_ || fault_steering_ || fault_steering_cal_ || fault_watchdog_; }
00127   inline bool override() { return override_brake_ || override_accelerator_pedal_ || override_steering_ || override_gear_; }
00128   inline bool clear() { return enable_ && override(); }
00129   inline bool enabled() { return enable_ && !fault() && !override(); }
00130   bool publishDbwEnabled();
00131   void enableSystem();
00132   void disableSystem();
00133   void buttonCancel();
00134   void overrideBrake(bool override);
00135   void overrideAcceleratorPedal(bool override);
00136   void overrideSteering(bool override);
00137   void overrideGear(bool override);
00138   void timeoutBrake(bool timeout, bool enabled);
00139   void timeoutAcceleratorPedal(bool timeout, bool enabled);
00140   void timeoutSteering(bool timeout, bool enabled);
00141   void faultBrakes(bool fault);
00142   void faultAcceleratorPedal(bool fault);
00143   void faultSteering(bool fault);
00144   void faultSteeringCal(bool fault);
00145   void faultWatchdog(bool fault, uint8_t src, bool braking);
00146   void faultWatchdog(bool fault, uint8_t src = 0);
00147 
00148   enum {
00149     JOINT_FL = 0, // Front left wheel
00150     JOINT_FR, // Front right wheel
00151     JOINT_RL, // Rear left wheel
00152     JOINT_RR, // Rear right wheel
00153     JOINT_SL, // Steering left
00154     JOINT_SR, // Steering right
00155     JOINT_COUNT, // Number of joints
00156   };
00157   sensor_msgs::JointState joint_state_;
00158   void publishJointStates(const ros::Time &stamp, const dbw_pacifica_msgs::WheelSpeedReport *wheels, const dbw_pacifica_msgs::SteeringReport *steering);
00159   // Licensing
00160   std::string vin_;
00161 
00162   // Frame ID
00163   std::string frame_id_;
00164 
00165   // Buttons (enable/disable)
00166   bool buttons_;
00167 
00168   // Ackermann steering
00169   double acker_wheelbase_;
00170   double acker_track_;
00171   double steering_ratio_;
00172 
00173   // Subscribed topics
00174   ros::Subscriber sub_enable_;
00175   ros::Subscriber sub_disable_;
00176   ros::Subscriber sub_can_;
00177   ros::Subscriber sub_brake_;
00178   ros::Subscriber sub_accelerator_pedal_;
00179   ros::Subscriber sub_steering_;
00180   ros::Subscriber sub_gear_;
00181   ros::Subscriber sub_misc_;
00182   ros::Subscriber sub_global_enable_;
00183 
00184   // Published topics
00185   ros::Publisher pub_can_;
00186   ros::Publisher pub_brake_;
00187   ros::Publisher pub_accel_pedal_;
00188   ros::Publisher pub_steering_;
00189   ros::Publisher pub_gear_;
00190   ros::Publisher pub_misc_;
00191   ros::Publisher pub_wheel_speeds_;
00192   ros::Publisher pub_wheel_positions_;
00193   ros::Publisher pub_tire_pressure_;
00194   ros::Publisher pub_surround_;
00195   ros::Publisher pub_imu_;
00196   ros::Publisher pub_joint_states_;
00197   ros::Publisher pub_twist_;
00198   ros::Publisher pub_vin_;
00199   ros::Publisher pub_sys_enable_;
00200   ros::Publisher pub_driver_input_;
00201   ros::Publisher pub_low_voltage_system_;
00202 
00203   ros::Publisher pub_brake_2_report_;
00204   ros::Publisher pub_steering_2_report_;
00205   ros::Publisher pub_fault_actions_report_;
00206   ros::Publisher pub_hmi_global_enable_report_;
00207 
00208   NewEagle::Dbc dbwDbc_;
00209   std::string dbcFile_;
00210 
00211   // Test stuff
00212   ros::Publisher pdu1_relay_pub_;
00213   uint32_t count_;
00214 };
00215 
00216 } // dbw_pacifica_can
00217 
00218 #endif // _DBW_NODE_H_
00219