├── .github └── workflows │ └── ros_test.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── decanstructor │ ├── common.h │ ├── decanstructor_node.h │ └── message_analyzer.h ├── launch └── decanstructor.launch ├── msg └── CanEvent.msg ├── package.xml ├── screenshots ├── decanstructor_1.png └── decanstructor_2.png └── src ├── decanstructor_node.cpp └── message_analyzer.cpp /.github/workflows/ros_test.yml: -------------------------------------------------------------------------------- 1 | name: ROS Test 2 | 3 | on: [push] 4 | 5 | jobs: 6 | melodic_build: 7 | runs-on: ubuntu-18.04 8 | env: 9 | CI_DISTRO: "`lsb_release -cs`" # e.g. [trusty|xenial|...] 10 | CI_SOURCE_PATH: $(pwd) 11 | ROS_PARALLEL_JOBS: '-j8 -l6' 12 | # Set the python path manually to include /usr/-/python2.7/dist-packages 13 | # as this is where apt-get installs python packages. 14 | PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages 15 | ROS_DISTRO: melodic 16 | steps: 17 | - uses: actions/checkout@v1 18 | - name: Install ROS 19 | run: | 20 | sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $CI_DISTRO main\" > /etc/apt/sources.list.d/ros-latest.list" 21 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 22 | sudo apt-get update -qq 23 | sudo apt-get install -y dpkg 24 | sudo apt-get install -y python-catkin-tools python-catkin-pkg python-rosdep python-wstool python-catkin-tools 25 | # Prepare rosdep to install dependencies. 26 | sudo rosdep init 27 | rosdep update 28 | rosdep install -y --from-paths . --ignore-src --rosdistro $ROS_DISTRO 29 | - name: Create Workspace 30 | run: | 31 | source /opt/ros/$ROS_DISTRO/setup.bash 32 | mkdir -p catkin_ws/src/decanstructor 33 | mv `find -maxdepth 1 -not -name . -not -name catkin_ws` catkin_ws/src/decanstructor/ 34 | cd catkin_ws 35 | catkin init 36 | - name: Build 37 | run: | 38 | source /opt/ros/$ROS_DISTRO/setup.bash 39 | cd catkin_ws 40 | catkin build 41 | kinetic_build: 42 | runs-on: ubuntu-16.04 43 | env: 44 | CI_DISTRO: "`lsb_release -cs`" # e.g. [trusty|xenial|...] 45 | CI_SOURCE_PATH: $(pwd) 46 | ROS_PARALLEL_JOBS: '-j8 -l6' 47 | # Set the python path manually to include /usr/-/python2.7/dist-packages 48 | # as this is where apt-get installs python packages. 49 | PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages 50 | ROS_DISTRO: kinetic 51 | steps: 52 | - uses: actions/checkout@v1 53 | - name: Install ROS 54 | run: | 55 | sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $CI_DISTRO main\" > /etc/apt/sources.list.d/ros-latest.list" 56 | sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 57 | sudo apt-get update -qq 58 | sudo apt-get install -y python-catkin-tools python-catkin-pkg python-rosdep python-wstool python-catkin-tools 59 | # Prepare rosdep to install dependencies. 60 | sudo rosdep init 61 | rosdep update 62 | rosdep install -y --from-paths . --ignore-src --rosdistro $ROS_DISTRO 63 | - name: Create Workspace 64 | run: | 65 | source /opt/ros/$ROS_DISTRO/setup.bash 66 | mkdir -p catkin_ws/src/decanstructor 67 | mv `find -maxdepth 1 -not -name . -not -name catkin_ws` catkin_ws/src/decanstructor/ 68 | cd catkin_ws 69 | catkin init 70 | - name: Build 71 | run: | 72 | source /opt/ros/$ROS_DISTRO/setup.bash 73 | cd catkin_ws 74 | catkin build 75 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | *.log 4 | *.swp 5 | *.user 6 | *~ 7 | .idea/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(decanstructor) 3 | aux_source_directory(. SRC_LIST) 4 | 5 | ## Compile as C++11, supported in ROS Kinetic and newer 6 | add_compile_options(-std=c++11) 7 | 8 | find_package(catkin REQUIRED 9 | roscpp 10 | can_msgs 11 | std_msgs 12 | message_generation 13 | ) 14 | 15 | set(wxWidgets_CONFIGURATION mswu) 16 | 17 | find_package(wxWidgets COMPONENTS 18 | core 19 | base 20 | adv 21 | REQUIRED 22 | ) 23 | 24 | add_message_files( 25 | FILES 26 | CanEvent.msg 27 | ) 28 | 29 | ## Generate added messages and services with any dependencies listed here 30 | generate_messages( 31 | DEPENDENCIES 32 | std_msgs 33 | ) 34 | 35 | catkin_package( 36 | CATKIN_DEPENDS 37 | roscpp 38 | can_msgs 39 | std_msgs 40 | message_runtime 41 | DEPENDS wxWidgets 42 | ) 43 | 44 | include_directories( 45 | include/${PROJECT_NAME} 46 | ${catkin_INCLUDE_DIRS} 47 | ) 48 | 49 | include("${wxWidgets_USE_FILE}") 50 | 51 | add_executable(${PROJECT_NAME}_node 52 | src/message_analyzer.cpp 53 | src/decanstructor_node.cpp 54 | ${SRC_LIST} 55 | ) 56 | 57 | add_dependencies( 58 | ${PROJECT_NAME}_node 59 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 60 | ${catkin_EXPORTED_TARGETS} 61 | ) 62 | 63 | target_link_libraries(${PROJECT_NAME}_node 64 | ${wxWidgets_LIBRARIES} 65 | ${catkin_LIBRARIES} 66 | ) 67 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2017 Joshua Whitley 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DeCANstructor # 2 | 3 | The definitive CAN analysis tool for ROS. Allows rapid prototyping, reverse-engineering, and monitoring of CAN-based systems. 4 | 5 | ## Usage ## 6 | 7 | By default, the node subscribes to the `can_in` topic with type can_msgs/Frame. 8 | 9 | ### Playback Mode ### 10 | 11 | For playback mode, set the `~playback` parameter to true. If using the launch file, `use_sim_time` is automatically set to true. If not using the launch file, make sure to set this parameter yourself to get accurate change highlighting. A rosbag must be playing with the --clock option for playback mode to work properly. When playback mode is disabled, you can emit events with the "Publish Event" button. While in plaback mode, recorded events will flash an "Event Published" message in the bottom-right of the window. 12 | 13 | ## Screenshots ## 14 | 15 | ![Screenshot 1](/screenshots/decanstructor_2.png?raw=true "screenshot 1") 16 | -------------------------------------------------------------------------------- /include/decanstructor/common.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_H 2 | #define COMMON_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #ifndef WX_PRECOMP 12 | #include 13 | #endif 14 | #include 15 | 16 | #include 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /include/decanstructor/decanstructor_node.h: -------------------------------------------------------------------------------- 1 | #ifndef DECANSTRUCTOR_NODE_H 2 | #define DECANSTRUCTOR_NODE_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace DeCANstructor 11 | { 12 | wxDECLARE_EVENT(wxEVT_CMD_UPDATE_MSGS, wxThreadEvent); 13 | 14 | enum 15 | { 16 | ID_BTN_MESSAGE_ANALYZER = 1, 17 | ID_BTN_UNCHECK_ALL, 18 | ID_BTN_CHECK_ALL, 19 | ID_BTN_PUBLISH_EVENT 20 | }; 21 | 22 | enum EventMode 23 | { 24 | REAL_TIME, 25 | PLAYBACK 26 | }; 27 | 28 | enum SortDirection 29 | { 30 | NONE, 31 | ASCENDING, 32 | DESCENDING 33 | }; 34 | 35 | struct CanMsgDetail 36 | { 37 | std::vector bytes; 38 | std::vector last_bytes; 39 | std::vector last_updated_ms; 40 | uint64_t time_rcvd_ms = 0; 41 | uint64_t time_last_rcvd_ms = 0; 42 | unsigned int avg_rate = 0; 43 | int grid_index = -1; 44 | bool hidden = false; 45 | }; 46 | 47 | struct CellUpdate 48 | { 49 | int row; 50 | int col; 51 | uint64_t time_diff; 52 | }; 53 | 54 | // TODO: Finish implementing custom grid table. 55 | class DCGridTable : 56 | public wxGridTableBase 57 | { 58 | int GetNumberRows(); 59 | int GetNumberCols(); 60 | wxString GetValue(int row, int col); 61 | void SetValue(int row, int col, const wxString &value); 62 | void Clear(); 63 | bool InsertRows(size_t pos=0, size_t numRows=1); 64 | bool AppendRows(size_t numRows=1); 65 | bool DeleteRows(size_t pos=0, size_t numRows=1); 66 | bool InsertCols(size_t pos=0, size_t numCols=1); 67 | bool AppendCols(size_t numCols=1); 68 | bool DeleteCols(size_t pos=0, size_t numCols=1); 69 | void SetRowLabelValue(int row, const wxString &); 70 | void SetColLabelValue(int col, const wxString &); 71 | wxString GetRowLabelValue(int row); 72 | wxString GetColLabelValue(int col); 73 | }; 74 | 75 | class DCOptions 76 | { 77 | public: 78 | static uint16_t fade_out_time_ms; 79 | static ros::Time one_day_ago; 80 | static EventMode event_mode; 81 | }; 82 | 83 | class DCRenderTimer : 84 | public wxTimer 85 | { 86 | public: 87 | void Notify(); 88 | }; 89 | 90 | class DCFrame : 91 | public wxFrame 92 | { 93 | public: 94 | DCFrame(const wxString& title, 95 | const wxPoint& pos, 96 | const wxSize& size); 97 | 98 | std::unique_ptr main_grid; 99 | std::unique_ptr message_analyzer_btn; 100 | std::unique_ptr selector_box; 101 | std::unique_ptr render_timer; 102 | std::unique_ptr event_panel; 103 | std::unique_ptr pub_event_btn; 104 | std::unique_ptr pub_event_txt; 105 | 106 | std::mutex main_grid_mut; 107 | std::mutex event_mut; 108 | bool got_new_event; 109 | uint64_t most_recent_event_time; 110 | bool new_grid_select; 111 | 112 | private: 113 | std::unique_ptr spinner; 114 | ros::Publisher event_pub; 115 | ros::Subscriber can_sub; 116 | ros::Subscriber event_sub; 117 | 118 | void RedrawMessages(); 119 | 120 | void OnCanMsg(const can_msgs::Frame::ConstPtr& msg); 121 | void OnExit(wxCommandEvent& event); 122 | void OnAbout(wxCommandEvent& event); 123 | void OnMsgsUpdate(wxThreadEvent& event); 124 | void OnMessageAnalyzerClick(wxCommandEvent& event); 125 | void OnSelectorBoxTick(wxCommandEvent& event); 126 | void OnUncheckAll(wxCommandEvent& event); 127 | void OnCheckAll(wxCommandEvent& event); 128 | void OnPublishEvent(wxCommandEvent& event); 129 | void OnEventPublished(const decanstructor::CanEvent::ConstPtr& msg); 130 | void OnGridSelect(wxGridEvent& event); 131 | void OnSortById(wxGridEvent& event); 132 | 133 | wxDECLARE_EVENT_TABLE(); 134 | }; 135 | 136 | class DCNode : 137 | public wxApp 138 | { 139 | public: 140 | virtual bool OnInit(); 141 | DCFrame* frame; 142 | std::map> rcvd_msgs; 143 | std::mutex rcvd_msgs_mut; 144 | }; 145 | 146 | wxBEGIN_EVENT_TABLE(DCFrame, wxFrame) 147 | EVT_MENU(wxID_EXIT, DCFrame::OnExit) 148 | EVT_MENU(wxID_ABOUT, DCFrame::OnAbout) 149 | EVT_BUTTON(ID_BTN_MESSAGE_ANALYZER, DCFrame::OnMessageAnalyzerClick) 150 | EVT_CHECKLISTBOX(wxID_ANY, DCFrame::OnSelectorBoxTick) 151 | EVT_BUTTON(ID_BTN_UNCHECK_ALL, DCFrame::OnUncheckAll) 152 | EVT_BUTTON(ID_BTN_CHECK_ALL, DCFrame::OnCheckAll) 153 | EVT_BUTTON(ID_BTN_PUBLISH_EVENT, DCFrame::OnPublishEvent) 154 | EVT_GRID_LABEL_LEFT_CLICK(DCFrame::OnGridSelect) 155 | EVT_GRID_CELL_LEFT_CLICK(DCFrame::OnGridSelect) 156 | EVT_GRID_COL_SORT(DCFrame::OnSortById) 157 | wxEND_EVENT_TABLE() 158 | 159 | wxDEFINE_EVENT(wxEVT_CMD_UPDATE_MSGS, wxThreadEvent); 160 | } 161 | 162 | wxIMPLEMENT_APP(DeCANstructor::DCNode); 163 | wxDECLARE_APP(DeCANstructor::DCNode); 164 | 165 | #endif 166 | -------------------------------------------------------------------------------- /include/decanstructor/message_analyzer.h: -------------------------------------------------------------------------------- 1 | #ifndef MESSAGE_ANALYZER_H 2 | #define MESSAGE_ANALYZER_H 3 | 4 | #include 5 | 6 | namespace DeCANstructor 7 | { 8 | class DCMessageAnalyzerFrame : 9 | public wxFrame 10 | { 11 | public: 12 | DCMessageAnalyzerFrame(wxWindow* parent, 13 | const wxString& title, 14 | const wxPoint& pos, 15 | const wxSize& size); 16 | }; 17 | } 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /launch/decanstructor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /msg/CanEvent.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string event_desc 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | decanstructor 4 | 0.0.1 5 | The decanstructor package 6 | Josh Whitley 7 | Josh Whitley 8 | 9 | MIT 10 | 11 | catkin 12 | 13 | message_generation 14 | 15 | roscpp 16 | can_msgs 17 | std_msgs 18 | wxwidgets 19 | 20 | message_runtime 21 | 22 | -------------------------------------------------------------------------------- /screenshots/decanstructor_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JWhitleyWork/decanstructor/f920a99293a4fcb6397c242a28109b6ee4903d0f/screenshots/decanstructor_1.png -------------------------------------------------------------------------------- /screenshots/decanstructor_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JWhitleyWork/decanstructor/f920a99293a4fcb6397c242a28109b6ee4903d0f/screenshots/decanstructor_2.png -------------------------------------------------------------------------------- /src/decanstructor_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace DeCANstructor; 4 | 5 | uint16_t DCOptions::fade_out_time_ms = 3000; 6 | ros::Time DCOptions::one_day_ago = ros::Time(); 7 | EventMode DCOptions::event_mode = REAL_TIME; 8 | 9 | void DCRenderTimer::Notify() 10 | { 11 | auto& local_grid = wxGetApp().frame->main_grid; 12 | auto& local_msgs = wxGetApp().rcvd_msgs; 13 | 14 | uint64_t ros_now_ms = (ros::Time::now() - DCOptions::one_day_ago).toNSec() / 1000000; 15 | std::vector cells_to_update; 16 | uint32_t grid_index = 0; 17 | 18 | std::lock_guard grid_mut(wxGetApp().frame->main_grid_mut); 19 | 20 | wxGetApp().rcvd_msgs_mut.lock(); // Lock messages 21 | 22 | if (local_grid->IsSortOrderAscending()) 23 | { 24 | for (auto it = local_msgs.begin(); it != local_msgs.end(); ++it) 25 | { 26 | if (it->second->grid_index > -1) 27 | { 28 | for (uint8_t i = 0; i < it->second->last_updated_ms.size(); i++) 29 | { 30 | uint64_t time_diff = ros_now_ms - it->second->last_updated_ms[i]; 31 | 32 | if (time_diff < DCOptions::fade_out_time_ms && !(it->second->hidden)) 33 | { 34 | CellUpdate cu; 35 | cu.row = grid_index; 36 | cu.col = i + 1; 37 | cu.time_diff = time_diff; 38 | cells_to_update.push_back(cu); 39 | } 40 | } 41 | 42 | if ((ros_now_ms - it->second->time_rcvd_ms) < DCOptions::fade_out_time_ms) 43 | local_grid->SetCellValue(grid_index, 9, wxString::Format(wxT("%u"), it->second->avg_rate)); 44 | 45 | grid_index++; 46 | } 47 | } 48 | } 49 | else 50 | { 51 | for (auto it = local_msgs.rbegin(); it != local_msgs.rend(); ++it) 52 | { 53 | if (it->second->grid_index > -1) 54 | { 55 | for (uint8_t i = 0; i < it->second->last_updated_ms.size(); i++) 56 | { 57 | uint64_t time_diff = ros_now_ms - it->second->last_updated_ms[i]; 58 | 59 | if (time_diff < DCOptions::fade_out_time_ms && !(it->second->hidden)) 60 | { 61 | CellUpdate cu; 62 | cu.row = grid_index; 63 | cu.col = i + 1; 64 | cu.time_diff = time_diff; 65 | cells_to_update.push_back(cu); 66 | } 67 | } 68 | 69 | if ((ros_now_ms - it->second->time_rcvd_ms) < DCOptions::fade_out_time_ms) 70 | local_grid->SetCellValue(grid_index, 9, wxString::Format(wxT("%u"), it->second->avg_rate)); 71 | 72 | grid_index++; 73 | } 74 | } 75 | } 76 | 77 | wxGetApp().rcvd_msgs_mut.unlock(); // Unlock messages 78 | 79 | for (auto it = cells_to_update.begin(); it != cells_to_update.end(); it++) 80 | { 81 | if (it->row >= 0 && it->row < local_grid->GetNumberRows()) 82 | { 83 | wxColour cell_color; 84 | wxColour text_color; 85 | float norm_time_interval = it->time_diff / (float)(DCOptions::fade_out_time_ms - 100); 86 | norm_time_interval = (norm_time_interval > 1.0) ? 1.0 : ((norm_time_interval < 0.0) ? 0.0 : norm_time_interval); 87 | uint8_t text_fade = (uint8_t)((1.0 - norm_time_interval) * 255.0); 88 | text_color.Set(text_fade, text_fade, text_fade); 89 | 90 | if (norm_time_interval < 0.5) 91 | { 92 | uint8_t green = (uint8_t)((norm_time_interval * 2.0) * 255.0); 93 | cell_color.Set(255, green, 0); 94 | } 95 | else 96 | { 97 | uint8_t blue = (uint8_t)(((norm_time_interval - 0.5) * 2.0) * 255.0); 98 | cell_color.Set(255, 255, blue); 99 | } 100 | 101 | local_grid->SetCellBackgroundColour(it->row, it->col, cell_color); 102 | local_grid->SetCellTextColour(it->row, it->col, text_color); 103 | } 104 | } 105 | 106 | if (DCOptions::event_mode == PLAYBACK) 107 | { 108 | std::lock_guard event_mut(wxGetApp().frame->event_mut); 109 | 110 | if (wxGetApp().frame->got_new_event) 111 | { 112 | uint64_t time_diff = ros_now_ms - wxGetApp().frame->most_recent_event_time; 113 | 114 | if (time_diff < DCOptions::fade_out_time_ms) 115 | { 116 | float norm_time_interval = time_diff / (float)(DCOptions::fade_out_time_ms - 100); 117 | norm_time_interval = (norm_time_interval > 1.0) ? 1.0 : ((norm_time_interval < 0.0) ? 0.0 : norm_time_interval); 118 | uint8_t color_fade = (uint8_t)(norm_time_interval * 255.0); 119 | 120 | wxColour panel_color; 121 | panel_color.Set(color_fade, 255, color_fade); 122 | wxGetApp().frame->event_panel->SetBackgroundColour(panel_color); 123 | 124 | wxColour text_color; 125 | text_color.Set(color_fade, color_fade, color_fade); 126 | wxGetApp().frame->pub_event_txt->SetForegroundColour(text_color); 127 | 128 | if (norm_time_interval == 1.0) 129 | { 130 | ROS_INFO("Event fade is done."); 131 | wxGetApp().frame->got_new_event = false; 132 | } 133 | } 134 | } 135 | } 136 | 137 | if (wxGetApp().frame->new_grid_select) 138 | { 139 | wxArrayInt selected_rows = wxGetApp().frame->main_grid->GetSelectedRows(); 140 | 141 | if (selected_rows.GetCount() == 1) 142 | wxGetApp().frame->message_analyzer_btn->Enable(true); 143 | else 144 | wxGetApp().frame->message_analyzer_btn->Enable(false); 145 | 146 | wxGetApp().frame->new_grid_select = false; 147 | } 148 | } 149 | 150 | DCFrame::DCFrame(const wxString& title, 151 | const wxPoint& pos, 152 | const wxSize& size) : 153 | got_new_event(false), 154 | most_recent_event_time(0), 155 | wxFrame(NULL, 156 | wxID_ANY, 157 | title, 158 | pos, 159 | size) 160 | { 161 | 162 | // Set up basic window properties 163 | wxMenu* menu_file = new wxMenu; 164 | menu_file->Append(wxID_EXIT); 165 | 166 | wxMenu* menu_help = new wxMenu; 167 | menu_help->Append(wxID_ABOUT); 168 | 169 | wxMenuBar* menu_bar = new wxMenuBar; 170 | menu_bar->Append(menu_file, "&File"); 171 | menu_bar->Append(menu_help, "&Help"); 172 | 173 | SetMenuBar(menu_bar); 174 | 175 | CreateStatusBar(); 176 | SetStatusText("Welcome to DeCANstructor."); 177 | 178 | // Create the sizers for the sub-widgets 179 | wxFlexGridSizer* main_sizer = new wxFlexGridSizer(4, 5, 5); 180 | wxBoxSizer* right_sizer = new wxBoxSizer(wxVERTICAL); 181 | wxBoxSizer* message_analyzer_sizer = new wxBoxSizer(wxHORIZONTAL); 182 | wxBoxSizer* chkbx_cntrl_sizer = new wxBoxSizer(wxHORIZONTAL); 183 | wxBoxSizer* event_sizer = new wxStaticBoxSizer(wxHORIZONTAL, this, "Events"); 184 | 185 | // Create basic flags 186 | wxSizerFlags no_flags; 187 | wxSizerFlags expand_flag; 188 | wxSizerFlags main_flags; 189 | 190 | expand_flag.Expand(); 191 | main_flags.Expand().Align(wxALIGN_TOP); 192 | 193 | // Main Sizer - 1st Row 194 | main_sizer->AddSpacer(5); 195 | main_sizer->AddSpacer(5); 196 | main_sizer->AddSpacer(5); 197 | 198 | // Main Sizer - 2nd Row 199 | main_sizer->AddSpacer(5); 200 | main_sizer->AddSpacer(5); 201 | 202 | // Create the main grid 203 | main_grid = std::unique_ptr(new wxGrid(this, -1, wxPoint(0, 0), wxSize(600, 250))); 204 | 205 | // TODO: Automatically resize grid columns based on available space on window resize. 206 | 207 | main_grid->CreateGrid(0, 10); 208 | main_grid->EnableEditing(false); 209 | main_grid->DisableDragColSize(); 210 | main_grid->DisableDragRowSize(); 211 | main_grid->DisableDragGridSize(); 212 | main_grid->SetSelectionMode(wxGrid::wxGridSelectRows); 213 | main_grid->SetColLabelSize(wxGRID_AUTOSIZE); 214 | main_grid->SetRowLabelSize(40); 215 | main_grid->SetSelectionBackground(wxColour(150, 150, 255)); 216 | 217 | main_grid->SetColLabelValue(0, "ID"); 218 | main_grid->SetColLabelValue(1, "0"); 219 | main_grid->SetColLabelValue(2, "1"); 220 | main_grid->SetColLabelValue(3, "2"); 221 | main_grid->SetColLabelValue(4, "3"); 222 | main_grid->SetColLabelValue(5, "4"); 223 | main_grid->SetColLabelValue(6, "5"); 224 | main_grid->SetColLabelValue(7, "6"); 225 | main_grid->SetColLabelValue(8, "7"); 226 | main_grid->SetColLabelValue(9, "Avg Rate (ms)"); 227 | 228 | main_grid->SetDefaultColSize(40); 229 | main_grid->SetColSize(0, 100); 230 | main_grid->SetColSize(9, 105); 231 | main_grid->SetColMinimalAcceptableWidth(40); 232 | main_grid->SetColMinimalWidth(0, 100); 233 | main_grid->SetColMinimalWidth(9, 105); 234 | main_grid->SetSortingColumn(0); 235 | 236 | main_grid->SetColFormatNumber(9); 237 | 238 | wxGridCellAttr* id_attr = new wxGridCellAttr(); 239 | id_attr->SetAlignment(wxALIGN_LEFT, wxALIGN_CENTER); 240 | 241 | main_grid->SetColAttr(0, id_attr); 242 | 243 | for (uint8_t i = 1; i < 9; i++) 244 | { 245 | wxGridCellAttr* byte_attr = new wxGridCellAttr(); 246 | byte_attr->SetAlignment(wxALIGN_CENTER, wxALIGN_CENTER); 247 | 248 | main_grid->SetColAttr(i, byte_attr); 249 | } 250 | 251 | main_sizer->Add(main_grid.get(), main_flags.Proportion(1)); 252 | 253 | // Create the Message Analyzer button 254 | message_analyzer_btn = std::unique_ptr(new wxButton()); 255 | message_analyzer_btn->Create(this, ID_BTN_MESSAGE_ANALYZER, "Analyze Message"); 256 | message_analyzer_btn->Enable(false); // Start disabled 257 | 258 | message_analyzer_sizer->Add(message_analyzer_btn.get(), expand_flag.Proportion(1)); 259 | 260 | right_sizer->Add(message_analyzer_sizer, expand_flag.Proportion(0)); 261 | right_sizer->AddSpacer(5); 262 | 263 | // Create the Control Buttons for the CAN ID Selection Box 264 | wxButton* uncheck_all_btn = new wxButton(); 265 | wxButton* check_all_btn = new wxButton(); 266 | 267 | uncheck_all_btn->Create(this, ID_BTN_UNCHECK_ALL, "Uncheck All"); 268 | check_all_btn->Create(this, ID_BTN_CHECK_ALL, "Check All"); 269 | 270 | chkbx_cntrl_sizer->Add(uncheck_all_btn, expand_flag.Proportion(1)); 271 | chkbx_cntrl_sizer->AddSpacer(5); 272 | chkbx_cntrl_sizer->Add(check_all_btn, expand_flag.Proportion(1)); 273 | 274 | right_sizer->Add(chkbx_cntrl_sizer, expand_flag.Proportion(0)); 275 | right_sizer->AddSpacer(5); 276 | 277 | // Create the CAN ID Selection Box 278 | selector_box = std::unique_ptr(new wxCheckListBox()); 279 | selector_box->Create(this, -1, wxPoint(0, 0), wxSize(200, 350)); 280 | 281 | right_sizer->Add(selector_box.get(), main_flags.Proportion(1)); 282 | 283 | right_sizer->AddSpacer(5); 284 | 285 | // Create the Event Control Box 286 | event_panel = std::unique_ptr(new wxPanel(this, wxID_ANY, wxDefaultPosition, wxSize(200, 50))); 287 | 288 | pub_event_btn = std::unique_ptr(new wxButton()); 289 | pub_event_txt = std::unique_ptr(new wxStaticText()); 290 | 291 | pub_event_btn->Create(event_panel.get(), ID_BTN_PUBLISH_EVENT, "Publish Event", wxPoint(0, 0), wxSize(200, 50)); 292 | pub_event_txt->Create(event_panel.get(), wxID_ANY, ""); 293 | pub_event_txt->SetLabelMarkup("Event Published"); 294 | pub_event_txt->SetForegroundColour(wxColour(255, 255, 255)); 295 | 296 | pub_event_txt->CentreOnParent(); 297 | 298 | event_sizer->Add(event_panel.get()); 299 | 300 | right_sizer->Add(event_sizer, expand_flag.Proportion(0)); 301 | 302 | main_sizer->Add(right_sizer, main_flags.Right()); 303 | 304 | main_sizer->AddSpacer(5); 305 | main_sizer->AddSpacer(5); 306 | 307 | // Main Sizer - 3rd Row 308 | main_sizer->AddSpacer(5); 309 | main_sizer->AddSpacer(5); 310 | main_sizer->AddSpacer(5); 311 | 312 | main_sizer->AddGrowableCol(1, 3); 313 | main_sizer->AddGrowableRow(1); 314 | 315 | Connect(wxID_ANY, wxEVT_CMD_UPDATE_MSGS, wxThreadEventHandler(DCFrame::OnMsgsUpdate), NULL, this); 316 | 317 | // Start the render update timer with a 10ms interval. 318 | render_timer = std::unique_ptr(new DCRenderTimer); 319 | render_timer->Start(100); 320 | 321 | // The ROS stuff 322 | ros::NodeHandle node_handle; 323 | ros::NodeHandle private_handle("~"); 324 | spinner = std::unique_ptr(new ros::AsyncSpinner(2)); 325 | bool playback_bool = false; 326 | 327 | if (!private_handle.getParam("playback", playback_bool)) 328 | playback_bool = false; 329 | 330 | if (playback_bool) 331 | DCOptions::event_mode = PLAYBACK; 332 | else 333 | DCOptions::event_mode = REAL_TIME; 334 | 335 | if (DCOptions::event_mode == PLAYBACK) 336 | { 337 | ROS_INFO("Waiting for playback to begin..."); 338 | 339 | while (ros::Time::now().nsec == 0 && ros::ok()); 340 | 341 | if (ros::Time::now().nsec == 0) 342 | { 343 | ROS_ERROR("Node shut down. No playback detected. Exiting..."); 344 | Close(true); 345 | return; 346 | } 347 | } 348 | else 349 | { 350 | while (ros::Time::now().nsec == 0); 351 | } 352 | 353 | DCOptions::one_day_ago = ros::Time::now() - ros::Duration(60 * 60 * 24); 354 | 355 | event_pub = node_handle.advertise("events", 20); 356 | can_sub = node_handle.subscribe("can_tx", 100, &DCFrame::OnCanMsg, this); 357 | 358 | if (DCOptions::event_mode == PLAYBACK) 359 | { 360 | ROS_INFO("Entering playback mode..."); 361 | event_sub = node_handle.subscribe("events", 20, &DCFrame::OnEventPublished, this); 362 | pub_event_btn->Hide(); 363 | } 364 | else 365 | { 366 | pub_event_txt->Hide(); 367 | } 368 | 369 | spinner->start(); 370 | 371 | // We want this to happen last after playback mode takes effect. 372 | SetSizerAndFit(main_sizer); 373 | } 374 | 375 | void DCFrame::RedrawMessages() 376 | { 377 | std::lock_guard msgs_mut(wxGetApp().rcvd_msgs_mut); 378 | std::lock_guard grid_mut(wxGetApp().frame->main_grid_mut); 379 | 380 | auto& local_grid = wxGetApp().frame->main_grid; 381 | auto local_table = local_grid->GetTable(); 382 | auto& local_msgs = wxGetApp().rcvd_msgs; 383 | 384 | if (local_table->GetNumberRows() > 0) 385 | local_table->DeleteRows(0, local_table->GetNumberRows()); 386 | 387 | int item_count = wxGetApp().frame->selector_box->GetCount(); 388 | 389 | if (item_count > 0) 390 | { 391 | for (int i = item_count - 1; i > -1; --i) 392 | { 393 | wxGetApp().frame->selector_box->Delete(i); 394 | } 395 | } 396 | 397 | if (local_grid->IsSortOrderAscending()) 398 | { 399 | for (auto it = local_msgs.begin(); it != local_msgs.end(); ++it) 400 | { 401 | local_table->AppendRows(); 402 | int row_index = local_table->GetNumberRows() - 1; 403 | 404 | local_table->SetValue(row_index, 0, wxString::Format(wxT("0x%03X"), it->first)); 405 | 406 | for (uint8_t i = 0; i < it->second->bytes.size(); i++) 407 | { 408 | local_table->SetValue(row_index, i + 1, wxString::Format(wxT("%02X"), it->second->bytes[i])); 409 | } 410 | 411 | local_table->SetValue(row_index, 9, wxString::Format(wxT("%u"), 0)); 412 | 413 | // Add to selector box 414 | int selector_index = selector_box->Append(wxString::Format(wxT("0x%03X"), it->first)); 415 | 416 | if (it->second->hidden) 417 | local_grid->HideRow(row_index); 418 | else 419 | selector_box->Check(selector_index); 420 | 421 | it->second->grid_index = row_index; 422 | } 423 | } 424 | else 425 | { 426 | for (auto it = local_msgs.rbegin(); it != local_msgs.rend(); ++it) 427 | { 428 | local_table->AppendRows(); 429 | int row_index = local_table->GetNumberRows() - 1; 430 | 431 | local_table->SetValue(row_index, 0, wxString::Format(wxT("0x%03X"), it->first)); 432 | 433 | for (uint8_t i = 0; i < it->second->bytes.size(); i++) 434 | { 435 | local_table->SetValue(row_index, i + 1, wxString::Format(wxT("%02X"), it->second->bytes[i])); 436 | } 437 | 438 | local_table->SetValue(row_index, 9, wxString::Format(wxT("%u"), 0)); 439 | 440 | // Add to selector box 441 | int selector_index = selector_box->Append(wxString::Format(wxT("0x%03X"), it->first)); 442 | 443 | if (it->second->hidden) 444 | local_grid->HideRow(row_index); 445 | else 446 | selector_box->Check(selector_index); 447 | 448 | it->second->grid_index = row_index; 449 | } 450 | } 451 | 452 | local_grid->ForceRefresh(); 453 | } 454 | 455 | void DCFrame::OnExit(wxCommandEvent& event) 456 | { 457 | ros::shutdown(); 458 | Close(true); 459 | } 460 | 461 | void DCFrame::OnAbout(wxCommandEvent& event) 462 | { 463 | wxMessageBox("Copyright 2017 Joshua Whitley, All Rights Reserved", "About DeCANstructor", wxOK | wxICON_INFORMATION ); 464 | } 465 | 466 | void DCFrame::OnMsgsUpdate(wxThreadEvent& event) 467 | { 468 | if (event.GetString() == "true") 469 | { 470 | // New message - Redraw the grid 471 | RedrawMessages(); 472 | } 473 | else if (event.GetString() == "false") 474 | { 475 | // Existing message - Just update the grid row 476 | std::lock_guard callback_mut(wxGetApp().rcvd_msgs_mut); 477 | auto& local_msgs = wxGetApp().rcvd_msgs; 478 | auto& local_grid = wxGetApp().frame->main_grid; 479 | std::shared_ptr found_msg = std::shared_ptr(local_msgs[event.GetInt()]); 480 | 481 | for (uint8_t i = 0; i < found_msg->bytes.size(); i++) 482 | { 483 | if (found_msg->bytes[i] != found_msg->last_bytes[i]) 484 | { 485 | main_grid->SetCellValue(found_msg->grid_index, i + 1, wxString::Format(wxT("%02X"), found_msg->bytes[i])); 486 | } 487 | } 488 | 489 | uint64_t time_diff = found_msg->time_rcvd_ms - found_msg->time_last_rcvd_ms; 490 | 491 | // The new time difference should be 492 | // less than 1 minute (to be safe) 493 | if (time_diff < 60000) 494 | { 495 | // Make sure we don't have an average already 496 | if (found_msg->avg_rate == 0) 497 | found_msg->avg_rate = (unsigned int)time_diff; 498 | else 499 | found_msg->avg_rate = (found_msg->avg_rate + (unsigned int)time_diff) / 2; 500 | } 501 | } 502 | } 503 | 504 | void DCFrame::OnMessageAnalyzerClick(wxCommandEvent& event) 505 | { 506 | // Set up Message Analyzer window 507 | DCMessageAnalyzerFrame* ma_frame = new DCMessageAnalyzerFrame(this, 508 | "Message Analyzer", 509 | wxPoint(100, 100), 510 | wxSize(250, 250)); 511 | ma_frame->Show(true); 512 | } 513 | 514 | void DCFrame::OnSelectorBoxTick(wxCommandEvent& event) 515 | { 516 | auto& local_selector_box = wxGetApp().frame->selector_box; 517 | 518 | if (local_selector_box->IsChecked(event.GetInt())) 519 | wxGetApp().frame->main_grid->ShowRow(event.GetInt()); 520 | else 521 | wxGetApp().frame->main_grid->HideRow(event.GetInt()); 522 | 523 | std::lock_guard selector_mut(wxGetApp().rcvd_msgs_mut); 524 | 525 | auto& local_msgs = wxGetApp().rcvd_msgs; 526 | 527 | for (auto it = local_msgs.begin(); it != local_msgs.end(); it++) 528 | { 529 | if (it->second->grid_index == event.GetInt()) 530 | it->second->hidden = !local_selector_box->IsChecked(event.GetInt()); 531 | } 532 | } 533 | 534 | void DCFrame::OnUncheckAll(wxCommandEvent& event) 535 | { 536 | std::lock_guard uncheck_mut(wxGetApp().rcvd_msgs_mut); 537 | auto& local_msgs = wxGetApp().rcvd_msgs; 538 | 539 | for (auto it = local_msgs.begin(); it != local_msgs.end(); it++) 540 | { 541 | it->second->hidden = true; 542 | wxGetApp().frame->main_grid->HideRow(it->second->grid_index); 543 | wxGetApp().frame->selector_box->Check(it->second->grid_index, false); 544 | } 545 | } 546 | 547 | void DCFrame::OnCheckAll(wxCommandEvent& event) 548 | { 549 | std::lock_guard check_mut(wxGetApp().rcvd_msgs_mut); 550 | auto& local_msgs = wxGetApp().rcvd_msgs; 551 | 552 | for (auto it = local_msgs.begin(); it != local_msgs.end(); it++) 553 | { 554 | it->second->hidden = false; 555 | wxGetApp().frame->main_grid->ShowRow(it->second->grid_index); 556 | wxGetApp().frame->selector_box->Check(it->second->grid_index, true); 557 | } 558 | } 559 | 560 | void DCFrame::OnPublishEvent(wxCommandEvent& event) 561 | { 562 | decanstructor::CanEvent event_msg; 563 | event_msg.event_desc = ""; 564 | event_msg.header.stamp = ros::Time::now(); 565 | 566 | event_pub.publish(event_msg); 567 | } 568 | 569 | void DCFrame::OnEventPublished(const decanstructor::CanEvent::ConstPtr& msg) 570 | { 571 | std::lock_guard event_mut(wxGetApp().frame->event_mut); 572 | wxGetApp().frame->got_new_event = true; 573 | wxGetApp().frame->most_recent_event_time = (ros::Time::now() - DCOptions::one_day_ago).toNSec() / 1000000; 574 | ROS_INFO("Got a new event."); 575 | } 576 | 577 | void DCFrame::OnCanMsg(const can_msgs::Frame::ConstPtr& msg) 578 | { 579 | std::lock_guard callback_mut(wxGetApp().rcvd_msgs_mut); 580 | 581 | auto& local_msgs = wxGetApp().rcvd_msgs; 582 | auto found_msg = local_msgs.find(msg->id); 583 | 584 | // Do the following to get reasonable numbers to play with. 585 | uint64_t ros_now_ms = (ros::Time::now() - DCOptions::one_day_ago).toNSec() / 1000000; 586 | 587 | // The int value is the CAN ID. 588 | // The string value (true or false) indicates if this is a new message. 589 | wxThreadEvent evt(wxEVT_CMD_UPDATE_MSGS); 590 | evt.SetInt(msg->id); 591 | 592 | if (found_msg != local_msgs.end()) 593 | { 594 | // The message has already been received. 595 | // Store the old bytes for later comparison. 596 | std::copy(found_msg->second->bytes.begin(), found_msg->second->bytes.end(), found_msg->second->last_bytes.begin()); 597 | 598 | for (uint8_t i = 0; i < msg->data.size(); i++) 599 | { 600 | if (msg->data[i] != found_msg->second->bytes[i]) 601 | { 602 | found_msg->second->bytes[i] = msg->data[i]; 603 | found_msg->second->last_updated_ms[i] = ros_now_ms; 604 | } 605 | } 606 | 607 | found_msg->second->time_last_rcvd_ms = found_msg->second->time_rcvd_ms; 608 | found_msg->second->time_rcvd_ms = ros_now_ms; 609 | 610 | evt.SetString("false"); 611 | } 612 | else 613 | { 614 | // This is a new message. 615 | std::shared_ptr new_msg = std::shared_ptr(new CanMsgDetail); 616 | 617 | for (uint8_t i = 0; i < msg->data.size(); i++) 618 | { 619 | new_msg->bytes.push_back(msg->data[i]); 620 | new_msg->last_bytes.push_back(msg->data[i]); 621 | new_msg->last_updated_ms.push_back(ros_now_ms); 622 | } 623 | 624 | new_msg->time_rcvd_ms = ros_now_ms; 625 | new_msg->time_last_rcvd_ms = 0; 626 | local_msgs.insert(std::make_pair(msg->id, new_msg)); 627 | 628 | evt.SetString("true"); 629 | } 630 | 631 | wxGetApp().frame->GetEventHandler()->QueueEvent(evt.Clone()); 632 | } 633 | 634 | void DCFrame::OnGridSelect(wxGridEvent& event) 635 | { 636 | if (event.GetRow() != -1) 637 | new_grid_select = true; 638 | 639 | event.Skip(); 640 | } 641 | 642 | void DCFrame::OnSortById(wxGridEvent& event) 643 | { 644 | if (event.GetCol() == 0) 645 | { 646 | auto& local_grid = wxGetApp().frame->main_grid; 647 | 648 | if (local_grid->IsSortOrderAscending()) 649 | // Switch to descending 650 | local_grid->SetSortingColumn(0, false); 651 | else 652 | // Switch back to ascending 653 | local_grid->SetSortingColumn(0, true); 654 | 655 | local_grid->GetGridColLabelWindow()->Update(); 656 | RedrawMessages(); 657 | } 658 | 659 | event.Skip(); 660 | } 661 | 662 | bool DCNode::OnInit() 663 | { 664 | // ROS Node init 665 | ros::init(wxGetApp().argc, wxGetApp().argv, "DeCANstructor"); 666 | 667 | // wxWidgets Init 668 | frame = new DCFrame("DeCANstructor", wxPoint(50, 50), wxSize(450, 340)); 669 | frame->Show(true); 670 | 671 | SetTopWindow(frame); 672 | 673 | return true; 674 | } 675 | -------------------------------------------------------------------------------- /src/message_analyzer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace DeCANstructor; 4 | 5 | DCMessageAnalyzerFrame::DCMessageAnalyzerFrame(wxWindow* parent, 6 | const wxString& title, 7 | const wxPoint& pos, 8 | const wxSize& size) : 9 | wxFrame(parent, 10 | wxID_ANY, 11 | title, 12 | pos, 13 | size) 14 | { 15 | } 16 | --------------------------------------------------------------------------------