cob_bms_driver_node.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
 
#ifndef COB_BMS_DRIVER_NODE_H
#define COB_BMS_DRIVER_NODE_H
 
#include <ros/ros.h>
#include <XmlRpcValue.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
 
#include <socketcan_interface/socketcan.h>
#include <socketcan_interface/threading.h>
 
struct BmsParameter
{
    unsigned int offset;
    unsigned int length;
 
    bool is_signed;
 
    ros::Publisher publisher;
 
    diagnostic_msgs::KeyValue kv;
 
    virtual ~BmsParameter() {}
 
    virtual void update(const can::Frame &f) = 0;
    virtual void advertise(ros::NodeHandle &nh, const std::string &topic) = 0;
 
    typedef boost::shared_ptr<BmsParameter> Ptr;
};
 
class CobBmsDriverNode
{
private:
    ros::NodeHandle nh_;
    ros::NodeHandle nh_priv_;
 
    typedef std::multimap<uint8_t, BmsParameter::Ptr> ConfigMap;
 
    //ROS parameters
    ConfigMap config_map_;  //holds all the information that is provided in the configuration file
    int poll_period_for_two_ids_in_ms_;
    std::string can_device_;
    int bms_id_to_poll_;
    ros::Timer updater_timer_;
 
    boost::mutex data_mutex_;
 
    //polling lists that contain CAN-ID(s) that are to be polled. Each CAN-ID corresponds to a group of BMS parameters
    std::vector<uint8_t> polling_list1_;
    std::vector<uint8_t> polling_list2_;
    std::vector<uint8_t>::iterator polling_list1_it_;
    std::vector<uint8_t>::iterator polling_list2_it_;
 
    //interface to send and recieve CAN frames
    can::ThreadedSocketCANInterface socketcan_interface_;
 
    //pointer to callback function to handle CAN frames from BMS
    can::CommInterface::FrameListenerConstSharedPtr frame_listener_;
 
    //diagnostics data received from BMS
    diagnostic_updater::DiagnosticStatusWrapper stat_;
 
    //function to get ROS parameters from parameter server
    bool getParams();
 
    //function to interpret the diagnostics XmlRpcValue and save data in config_map_
    bool loadConfigMap(XmlRpc::XmlRpcValue &diagnostics, std::vector<std::string> &topics);
 
    //helper function to evaluate poll period from given poll frequency
    void evaluatePollPeriodFrom(int poll_frequency);
 
    //function that goes through config_map_ and fills polling_list1_ and polling_list2_ with CAN-IDs.
    //If Topics are found on ROS Parameter Server, they are kept in a separate list (to be polled faster).
    //Otherwise, all CAN-ID are divided between both lists.
    void optimizePollingLists();
 
    //function that polls BMS (bms_id_to_poll_ is used here!).
    //Also, this function sleeps for time given by poll_period_for_two_ids_in_ms_ to ensure BMS is polled at the desired frequency
    void pollBmsForIds(const uint16_t first_id, const uint16_t second_id);
 
    //callback function to handle all types of frames received from BMS
    void handleFrames(const can::Frame &f);
 
    //updates the diagnostics data with the new data received from BMS
    void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);
 
    //calls update function of diagnostics_updater
    void diagnosticsTimerCallback(const ros::TimerEvent&);
public:
 
    //updater for diagnostics data
    diagnostic_updater::Updater updater_;
 
    CobBmsDriverNode();
    ~CobBmsDriverNode();
 
    //initlializes SocketCAN interface, saves data from ROS parameter server, loads polling lists and sets up diagnostic updater
    bool prepare();
 
    //cycles through polling lists and sends 2 ids at a time (one from each list) to the BMS
    void pollNextInLists();
};
 
 
#endif //COB_BMS_DRIVER_NODE_H