From f6c49bff76ee9aa4d1aa6cde598de54e87890da4 Mon Sep 17 00:00:00 2001 From: David Date: Thu, 3 Nov 2022 11:47:52 +0000 Subject: [PATCH] First! --- .gitignore | 4 + CMakeLists.txt | 35 ++++++++ data.dbc | 60 +++++++++++++ include/can_wrap.h | 193 ++++++++++++++++++++++++++++++++++++++++ src/demo.cpp | 30 +++++++ src/demo_dbc.cpp | 88 ++++++++++++++++++ src/demo_dbc.py | 77 ++++++++++++++++ src/demo_raw_decode.cpp | 92 +++++++++++++++++++ src/demo_send.cpp | 56 ++++++++++++ 9 files changed, 635 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 data.dbc create mode 100644 include/can_wrap.h create mode 100644 src/demo.cpp create mode 100644 src/demo_dbc.cpp create mode 100644 src/demo_dbc.py create mode 100644 src/demo_raw_decode.cpp create mode 100644 src/demo_send.cpp diff --git a/.gitignore b/.gitignore index 46f42f8..54eb3b0 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,7 @@ install_manifest.txt compile_commands.json CTestTestfile.cmake _deps + +bin/ +src_autogen/ +lib/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..b3f8a2d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.0) +set(CMAKE_CXX_STANDARD 17) + +project(dbc_example C CXX) + +# location of source code files +include_directories( include ) + +# tell cmake where to put the executables that it creates +file( MAKE_DIRECTORY bin ) +set( CMAKE_RUNTIME_OUTPUT_DIRECTORY bin ) + +# where to put the object files it creates +file( MAKE_DIRECTORY lib ) +SET( LIBRARY_OUTPUT_PATH lib ) + +# autogenerate code from the DBC file +file( MAKE_DIRECTORY src_autogen ) +add_custom_command( OUTPUT src_autogen/data.h src_autogen/data.c + COMMAND python3 -m cantools generate_c_source data.dbc -o src_autogen + MAIN_DEPENDENCY data.dbc + COMMENT "Autogenerate code from DBC file" ) + +# create shared library of the autogenerated code from the DBC file +add_library( data SHARED src_autogen/data.c ) +target_include_directories( data PUBLIC src_autogen ) + +add_executable( demo_dbc src/demo_dbc.cpp ) +target_link_libraries( demo_dbc PRIVATE data ) + +add_executable( demo src/demo.cpp ) + +add_executable( demo_raw_decode src/demo_raw_decode.cpp ) + +add_executable( demo_send src/demo_send.cpp ) diff --git a/data.dbc b/data.dbc new file mode 100644 index 0000000..91c187d --- /dev/null +++ b/data.dbc @@ -0,0 +1,60 @@ +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: Auto VCU +BU_: Auto DASH + +BO_ 392 indicator_control: 4 DASH + SG_ Right_indicator : 1|1@1+ (1,0) [0|1] "" Vector__XXX + SG_ Left_indicator : 0|1@1+ (1,0) [0|1] "" Vector__XXX + +VAL_ 392 Right_indicator 0 "OFF" 1 "ON"; +VAL_ 392 Left_indicator 0 "OFF" 1 "ON"; + +BO_ 580 speed: 5 ECU + SG_ Speed : 31|16@0+ (1,0) [0|48280] "dam/h" Vector__XXX + +CM_ "SG_ Speed : 31|16@0+ (1,0) [0|48280] \"dam/h\" Vector__XXX +Is an example using decameters per hour. This is the format of the raw +data actually sent over the CAN bus. +Dam/h is not, however, a commonly used unit of measurement and so the +following message specification might be preferable. The scale factor +is used to adjust the raw values. +SG_ Speed : 31|16@0+ (0.01,0) [0|482.8] \"km/h\" Vector__XXX"; + + +BA_DEF_ "BusType" STRING ; +BA_DEF_DEF_ "BusType" "CAN"; + diff --git a/include/can_wrap.h b/include/can_wrap.h new file mode 100644 index 0000000..f907251 --- /dev/null +++ b/include/can_wrap.h @@ -0,0 +1,193 @@ +#ifndef CAN_WRAP_H +#define CAN_WRAP_H + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace can +{ + /** + * @brief Opens socket for CAN bus communication via socketcan. + * Returns socket identifier which is used as an argument for + * future can::read(), can::write() and can::close() calls. + * + * @throws std::runtime_error If socket creation failed. + * @throws std::runtime_error If could not enable CANFD. + * @throws std::runtime_error If could not bind socket. + * + * @param fd Open as normal CAN or CANFD. + * @param channel Network channel to use, e.g. can0 or vcan0. + * @return const int Socket identifier. + */ + const int _connect( const bool fd, const std::string& channel ) + { + const int canSocket = socket( PF_CAN, SOCK_RAW, CAN_RAW ); + if( canSocket < 0 ) + throw std::runtime_error( "Create socket failed" ); + + struct ifreq ifr; + std::strncpy( ifr.ifr_name, channel.c_str(), sizeof(ifr.ifr_name)-1 ); + ioctl( canSocket, SIOCGIFINDEX, &ifr ); + + struct sockaddr_can addr; + std::memset( &addr, 0, sizeof(addr) ); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + int enableCanFd = 1; + if( fd && setsockopt( canSocket, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enableCanFd, sizeof(enableCanFd) ) ) + throw std::runtime_error( "Enable CAN FD failed" ); + + if( bind( canSocket, reinterpret_cast(&addr), sizeof(addr) ) < 0 ) + throw std::runtime_error( "Bind socket failed" ); + + return canSocket; + } + + /** + * @brief Passes call to can::_connect with fd argument as false. + */ + inline const int connect( const std::string channel="vcan0" ) + { + return _connect( false, channel ); + } + + /** + * @brief Passes call to can::_connect with fd argument as true. + */ + inline const int connectfd( const std::string channel="vcan0" ) + { + return _connect( true, channel ); + } + + /** + * @brief Close specified socketcan socket. + * + * @throws std::runtime_error If close failed. + * + * @param socket Socket identifer as returned from can::connect(). + */ + void close( const int socket ) + { + if( ::close( socket ) < 0 ) + throw std::runtime_error( "Socket close failed" ); + } + + /** + * @brief Read next frame from CAN bus. + * + * @warning This function will block until a frame is available. + * + * @throws std::runtime_error If read failed. + * + * @param socket Socket identifer as returned from can::connect(). + * @return const can_frame + */ + template + const FRAME _read( const int socket ) + { + FRAME frame; + + if( read( socket, &frame, sizeof(frame) ) < 0 ) + throw std::runtime_error( "Read failed" ); + + return frame; + } + + /** + * @brief Passes call to can::_read() to read standard CAN frame. + * + * @param socket Socket identifer as returned from can::connect(). + * @return const can_frame + */ + inline const can_frame read( const int socket ) + { + return _read( socket ); + } + + /** + * @brief Passes call to can::_read() to read CANFD frame. + * + * @param socket Socket identifer as returned from can::connect(). + * @return const canfd_frame + */ + inline const canfd_frame readfd( const int socket ) + { + return _read( socket ); + } + + /** + * @brief Send CAN frame. + * + * @throws std::runtime_error If write failed. + * + * @tparam FRAME can_frame or canfd_frame + * @param socket Socket identifer as returned from can::connect(). + * @param frame CAN or CANFD frame to be sent. + * @param numBytes Number of bytes to be written. + */ + template + void _write( const int socket, const FRAME& frame, const uint8_t numBytes ) + { + if( write( socket, &frame, numBytes ) != numBytes ) + std::runtime_error( "Write failed" ); + } + + /** + * @brief Passes call to can::_write() to write standard CAN frame. + * + * @param socket Socket identifer as returned from can::connect(). + * @param frame CAN frame to be sent. + */ + void write( const int socket, const can_frame& frame ) + { + // 8 bytes for the frame header and the rest for the actual data. + _write( socket, frame, sizeof(can_frame) ); + } + + /** + * @brief Passes call to can::_write() to write CANFD frame. + * + * @param socket Socket identifer as returned from can::connect(). + * @param frame CANFD frame to be sent. + */ + void write( const int socket, const canfd_frame& frame ) + { + // 8 bytes for the frame header and the rest for the actual data. + _write( socket, frame, sizeof(canfd_frame) ); + } + + + std::ostream& operator<<( std::ostream &os, const can_frame& frame ) + { + os << "0x" << std::setw(3) << frame.can_id << + " [" << static_cast(frame.can_dlc) << ']'; + for( int i=0; i(frame.data[i]); + + return os; + } + + std::ostream& operator<<( std::ostream &os, const canfd_frame& frame ) + { + os << "0x" << std::setw(3) << frame.can_id << + " [" << static_cast(frame.len) << ']'; + for( int i=0; i(frame.data[i]); + + return os; + } +} + +#endif diff --git a/src/demo.cpp b/src/demo.cpp new file mode 100644 index 0000000..755f5a6 --- /dev/null +++ b/src/demo.cpp @@ -0,0 +1,30 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include "can_wrap.h" +using can::operator<<; + +int main( int argc, char* argv[] ) +{ + const std::string canChannel = "vcan0"; + const int canSocket = can::connect( canChannel ); + + std::cout << std::hex << std::uppercase << std::setfill('0'); + + for( int c=0; c<100; ++c ) + { + auto frame = can::read( canSocket ); + + std::cout << frame << std::endl; + } + + can::close( canSocket ); + + return 0; +} \ No newline at end of file diff --git a/src/demo_dbc.cpp b/src/demo_dbc.cpp new file mode 100644 index 0000000..c6ee8e4 --- /dev/null +++ b/src/demo_dbc.cpp @@ -0,0 +1,88 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include "can_wrap.h" +using can::operator<<; + +#include "data.h" + +void indicator_frame( const can_frame frame ) +{ + data_indicator_control_t data; + data_indicator_control_unpack( &data, frame.data, frame.can_dlc ); + + if( !data_indicator_control_left_indicator_is_in_range( data.left_indicator ) ) + throw std::out_of_range( "left" ); + + if( !data_indicator_control_right_indicator_is_in_range( data.right_indicator ) ) + throw std::out_of_range( "right" ); + + const bool left = data_indicator_control_left_indicator_decode( data.left_indicator ); + const bool right = data_indicator_control_right_indicator_decode( data.right_indicator ); + + if( !left && !right ) + std::cout << "Indicator off" << std::endl; + else + { + std::cout << "Indicator on"; + + if( left ) + std::cout << ", left"; + + if( right ) + std::cout << ", right"; + + std::cout << std::endl; + } +} + +void speed_frame( const can_frame frame ) +{ + data_speed_t data; + data_speed_unpack( &data, frame.data, frame.can_dlc ); + + if( !data_speed_speed_is_in_range( data.speed ) ) + throw std::out_of_range( "speed" ); + + const float km = data_speed_speed_decode( data.speed ); + + std::cout << "Speed, " << km << "km/h" << std::endl; +} + +void process_frame( const can_frame frame ) +{ + switch( frame.can_id ) + { + case DATA_INDICATOR_CONTROL_FRAME_ID: + indicator_frame( frame ); + break; + case DATA_SPEED_FRAME_ID: + speed_frame( frame ); + break; + default: + /* unknown frame */ + break; + } +} + +int main( int argc, char* argv[] ) +{ + const std::string canChannel = "vcan0"; + const int canSocket = can::connect( canChannel ); + + for( int c=0; c<100000; ++c ) + { + const can_frame frame = can::read( canSocket ); + process_frame( frame ); + } + + can::close( canSocket ); + + return 0; +} diff --git a/src/demo_dbc.py b/src/demo_dbc.py new file mode 100644 index 0000000..8588205 --- /dev/null +++ b/src/demo_dbc.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 + +import sys + +import can +import cantools +import os + +dbcFilename = "data.dbc" +dbcPath = os.path.join( dbcFilename ) + +canType = "vcan0" +canChannel = "socketcan" + +def main(): + db = cantools.database.load_file( dbcPath ) + + # === open can device === + try: + bus = can.interface.Bus( channel=canChannel, bustype=canType, + can_filters=filters ) + except OSError: + rospy.logerr( f"Failed to open {canChannel}" ) + return 1 + + # === main loop === + while not rospy.is_shutdown(): + # get can message + msg = bus.recv( 1 ) + if msg is None: # nothing recieved + continue + + if msg.arbitration_id not in pubs: continue # why does the filter let some stuff through? + + # set up stamped message + for stamped, dbc, pub in pubs[msg.arbitration_id]: + rospy.logdebug( dbc.name ) + + # decode raw can bytes to dictionary + try: + data = dbc.decode( msg.data ) + except ValueError: + rospy.logwarn( f"{dbc.name} decoding failed" ) + + rospy.logdebug( f"{dbc.name} {data}" ) + + for k, v in data.items(): + if isinstance(v, cantools.database.can.signal.NamedSignalValue): + data[k] = v.value + + rospy.logdebug( f"{dbc.name} {data}" ) + + if stamped: + msgType = f"pix_driver/{dbc.name}_stamped" + data = { "data": data } + else: + msgType = f"pix_driver/{dbc.name}" + + message = message_converter.convert_dictionary_to_ros_message( msgType, data, check_types=False, check_missing_fields=False ) + + if stamped: + message.header.stamp = rospy.Time.now() + + pub.publish( message ) + + #messageStamped = p.data_class() + #messageStamped.header.stamp = rospy.Time.now() + + # convert decoded dictionary to ros message + #messageStamped.data = message_converter.convert_dictionary_to_ros_message( f"pix_driver/{dbc.name}", data, check_types=False, check_missing_fields=True ) + + #pub.publish( messageStamped ) + + bus.shutdown() + +if __name__ == '__main__': + sys.exit( main() ) diff --git a/src/demo_raw_decode.cpp b/src/demo_raw_decode.cpp new file mode 100644 index 0000000..4ed4303 --- /dev/null +++ b/src/demo_raw_decode.cpp @@ -0,0 +1,92 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include "can_wrap.h" +using can::operator<<; + +void indicator_frame( const can_frame frame ) +{ + static const uint8_t indicatorSignalMax = 1; + // static const uint8_t indicatorSignalMin = 0; + + const uint8_t leftSignal = frame.data[0] & 0b00000001; + const uint8_t rightSignal = frame.data[0] & 0b00000010; + + if( leftSignal > indicatorSignalMax ) throw std::out_of_range( "Left over range" ); + if( rightSignal > indicatorSignalMax ) throw std::out_of_range( "Right over range" ); + // if( leftSignal < indicatorSignalMin ) throw std::out_of_range( "Left under range" ); + // if( rightSignal < indicatorSignalMin ) throw std::out_of_range( "Right under range" ); + + const bool left = leftSignal; + const bool right = rightSignal; + + if( !left && !right ) + std::cout << "Indicator off" << std::endl; + else + { + std::cout << "Indicator on"; + + if( left ) + std::cout << ", left"; + + if( right ) + std::cout << ", right"; + + std::cout << std::endl; + } +} + +void speed_frame( const can_frame frame ) +{ + static const float damToKm = 0.01f; + static const uint16_t speedSignalMax = 60000; + // static const uint16_t speedSignalMin = 0; + + const uint16_t speedSignal = static_cast( frame.data[3] << 8 ) + + static_cast( frame.data[4] ); + + if( speedSignal > speedSignalMax ) throw std::out_of_range( "Speed over range" ); + // if( speedSignal < speedSignalMin ) throw std::out_of_range( "Speed under range" ); + + const float km = speedSignal * damToKm; + + std::cout << "Speed, " << km << "km/h" << std::endl; +} + +void process_frame( const can_frame frame ) +{ + switch( frame.can_id ) + { + case 0x188: + indicator_frame( frame ); + break; + case 0x244: + speed_frame( frame ); + break; + default: + /* unknown frame */ + break; + } +} + +int main( int argc, char* argv[] ) +{ + const std::string canChannel = "vcan0"; + const int canSocket = can::connect( canChannel ); + + for( int c=0; c<100000; ++c ) + { + const can_frame frame = can::read( canSocket ); + process_frame( frame ); + } + + can::close( canSocket ); + + return 0; +} \ No newline at end of file diff --git a/src/demo_send.cpp b/src/demo_send.cpp new file mode 100644 index 0000000..d2c343c --- /dev/null +++ b/src/demo_send.cpp @@ -0,0 +1,56 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "can_wrap.h" +using can::operator<<; + +int main( int argc, char* argv[] ) +{ + const std::string canChannel = "vcan0"; + const int canSocket = can::connect( canChannel ); + + /*std::cout << std::hex << std::uppercase << std::setfill('0'); + + for( int c=0; c<100; ++c ) + { + auto frame = can::read( canSocket ); + + std::cout << frame << std::endl; + } +*/ + + bool state = false; + for( int c=0; c<100; ++c ) + { + can_frame frame; + std::memset( &frame, 0, sizeof(frame) ); + + frame.can_id = 0x188; + frame.can_dlc = 4; + + switch( c%4 ) + { + case 0: frame.data[0] = 0b00000000; break; + case 1: frame.data[0] = 0b00000001; break; + case 2: frame.data[0] = 0b00000010; break; + default: frame.data[0] = 0b00000011; break; + } + + if( write( canSocket, &frame, sizeof(frame) ) != sizeof(frame) ) + std::cerr << "write fail" << std::endl; + + std::this_thread::sleep_for( std::chrono::seconds( 1 ) ); + } + + can::close( canSocket ); + + return 0; +}