Wallfollowing improvements

ros_ws/launch/car_wallfollowing.launch

@@ -44,6 +44,8 @@
     <!-- wallfollowing start - please enable only one -->
     <!-- current implementation -->
     <include file="$(find wallfollowing5)/launch/autonomous_driving_physical.launch"/>
+
+    <node pkg="car_config" type="dynamic_physical_properties" name="dynamic_physical_properties" respawn="true" output="screen"/>
 
     <!-- former implementation of previous group -->
     <!--<include file="$(find wallfollowing2)/launch/autonomous_driving_physical.launch"/>-->
@@ -102,14 +104,19 @@
     </group>
 
     <!-- publishes the approximated maximal possible speed for a certain position on the track -->
-    <!--<node pkg="simulation_tools" type="speed_info" name="speed_info" respawn="true" output="screen"/>-->
+    <node pkg="analysis-tools" type="speed_info" name="speed_info" respawn="true" output="screen"/>
 
     <!-- needed for logging telemetry data and provide node for rviz -->
-    <!-- arguments -->
-    <!-- 1: log prefix (string), default: none -->
-    <!-- 2: length for min/max arrays (int), default: 100 -->
-    <!-- 3: smoothing value of acceleration (int), default: 5 -->
-    <!-- 4: whether its simulation or not (string: yes|no), default: no -->
-    <!-- 5: whether write statistics or not (string: yes|no), default: no -->
-    <!--<node pkg="simulation_tools" type="log_stats.py" name="log_stats" output="screen" args="wallfollowing 100 10 no yes"/>-->
+    <!-- prefix: log prefix (string), default: none -->
+    <!-- length: length for min/max arrays (int), default: 100 -->
+    <!-- smooth: smoothing value of acceleration (int), default: 5 -->
+    <!-- sim: whether its simulation or not (bool), default: false -->
+    <!-- write: whether write statistics or not (bool), default: false -->
+    <node pkg="analysis-tools" type="log_stats" name="log_stats" respawn="true" output="screen">
+        <param name="prefix" value="wallfollowing" />
+        <param name="length" value="100" />
+        <param name="smooth" value="10" />
+        <param name="sim" value="false" />
+        <param name="write" value="true" />
+    </node>
 </launch>

ros_ws/launch/gazebo.launch

@@ -126,6 +126,8 @@
 
     <include file="$(find imu_velocity)/launch/car_imu_velocity.launch" />
 
+    <node pkg="car_config" type="dynamic_physical_properties" name="dynamic_physical_properties" respawn="true" output="screen"/>
+
     <node pkg="simulation_tools" type="crash_detector" name="crash_detector" output="screen">
     </node>
     <node pkg="tf" type="static_transform_publisher" name="depth_rotation" args="0.18 0 0.14 -1.570796327 0 -1.570796327 /base_link /camera 10" />
@@ -134,13 +136,18 @@
     <node pkg="simulation_tools" type="speed_info" name="speed_info" respawn="true" output="screen"/>
 
     <!-- needed for logging telemetry data and provide node for rviz -->
-    <!-- arguments -->
-    <!-- 1: log prefix (string), default: none -->
-    <!-- 2: length for min/max arrays (int), default: 100 -->
-    <!-- 3: smoothing value of acceleration (int), default: 5 -->
-    <!-- 4: whether its simulation or not (string: yes|no), default: no -->
-    <!-- 5: whether write statistics or not (string: yes|no), default: no -->
-    <node pkg="simulation_tools" type="log_stats.py" name="log_stats" respawn="true" output="screen" args="gazebo 100 10 yes yes"/>
+    <!-- prefix: log prefix (string), default: none -->
+    <!-- length: length for min/max arrays (int), default: 100 -->
+    <!-- smooth: smoothing value of acceleration (int), default: 5 -->
+    <!-- sim: whether its simulation or not (bool), default: false -->
+    <!-- write: whether write statistics or not (bool), default: false -->
+    <node pkg="analysis-tools" type="log_stats" name="log_stats" respawn="true" output="screen">
+        <param name="prefix" value="gazebo" />
+        <param name="length" value="100" />
+        <param name="smooth" value="10" />
+        <param name="sim" value="true" />
+        <param name="write" value="true" />
+    </node>
 
     <group if="$(arg record)">
         <node pkg="image_view" type="video_recorder" name="video_record_cam">

ros_ws/src/analysis-tools/CMakeLists.txt

@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(analysis-tools)
+
+## Compile as C++14, supported in ROS Melodic and newer
+add_compile_options(-std=c++14)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  drive_msgs
+  geometric_math
+  std_msgs
+  gazebo_msgs
+  car_config
+  vesc_sim
+  geometric_math
+)
+
+
+## Errors and Warnings
+set ( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wchar-subscripts -Wchkp -Wformat -Wnonnull -Wmain -Wswitch-bool -Winvalid-memory-model -Wunknown-pragmas -Warray-bounds -Wfloat-equal -Wlogical-op -Wpacked ")
+# -Wpedantic cant be used because of ROS
+
+###################################
+## catkin specific configuration ##
+###################################
+
+## Declare things to be passed to dependent projects
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS roscpp drive_msgs std_msgs car_config vesc_sim geometric_math
+)
+
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+add_executable(log_stats   
+  src/log_stats.cpp
+)
+
+target_link_libraries(log_stats ${catkin_LIBRARIES})
+add_dependencies(log_stats ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+# Declare speed info node executable
+add_executable(speed_info
+  src/speed_info.cpp
+  src/process_track.cpp
+)
+target_link_libraries(speed_info ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+add_dependencies(speed_info ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

ros_ws/src/analysis-tools/include/config.h

@@ -0,0 +1,10 @@
+#pragma once
+
+namespace Config
+{
+    static const float USABLE_LASER_RANGE = 220;
+    static const double MAX_STEERING_ANGLE = 40.0f;
+    static const double RPM_FACTOR = 1299.224f;
+    static const double LOG_INTERVAL = 0.05f;
+    static const double HUD_PRECISION = 4U;
+} // namespace Config

ros_ws/src/analysis-tools/include/log_stats.h

@@ -0,0 +1,130 @@
+#pragma once
+
+#include "config.h"
+#include <ctime>
+#include <deque>
+#include <drive_msgs/drive_param.h>
+#include <drive_msgs/gazebo_state_telemetry.h>
+#include <fstream>
+#include <iostream>
+#include <jsk_rviz_plugins/OverlayText.h>
+#include <ros/package.h>
+#include <ros/ros.h>
+#include <sstream>
+#include <std_msgs/ColorRGBA.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float64.h>
+#include <sys/stat.h>
+
+constexpr const char* TOPIC_CONTROLLED_DRIVE_PARAM = "/commands/controlled_drive_param";
+constexpr const char* TOPIC_MAX_SPEED = "/speed_info/max_speed";
+constexpr const char* TOPIC_GAZEBO_STATE_TELEMETRY = "/gazebo/state_telemetry";
+
+class LogStats
+{
+    private:
+    ros::NodeHandle m_node_handle;
+    ros::Subscriber m_controlled_drive_parameters_subscriber;
+    ros::Subscriber m_max_speed_subscriber;
+    ros::Subscriber m_gazebo_state_telemetry_subscriber;
+    ros::Publisher m_hud_speed_publisher;
+    ros::Publisher m_hud_maxspeed_publisher;
+    ros::Publisher m_hud_rpm_publisher;
+    ros::Publisher m_hud_acceleration_publisher;
+    ros::Publisher m_hud_angle_publisher;
+    ros::Publisher m_hud_distance_publisher;
+    ros::Publisher m_hud_clock_publisher;
+    ros::Publisher m_hud_text_publisher;
+
+    // paths and filenames
+    std::string m_filenamedat;
+    std::string m_filenamecsv;
+
+    // create file handlers
+    std::ofstream m_filestream_dat;
+    std::ofstream m_filestream_csv;
+
+    // counter for logentry
+    unsigned int m_logentry;
+
+    // current time
+    ros::Time m_time_start;
+    ros::Duration m_time_last;
+    ros::Duration m_time_delta;
+    ros::Duration m_time_interval;
+    ros::Duration m_time_current;
+
+    // parameters
+    std::string m_log_prefix;
+    int m_mean_length;
+    int m_smooth_count;
+    bool m_is_simulation;
+    bool m_write_logfile;
+
+    // incoming data
+    // double m_current_speed;
+    // double m_current_steering_angle;
+    // double m_max_speed;
+    // double m_gazebo_wheel_speed; // ???
+    // double m_gazebo_car_speed; // ???
+
+    // calculated data - speed
+    double m_speed_current;
+    double m_speed_last;
+    double m_speed_delta;
+    double m_speed_avg;
+    std::deque<double> m_speed_avgtime;
+    double m_speed_max;
+    std::deque<double> m_speed_maxtime;
+    std::deque<double> m_speed_smooth;
+
+    // calculated data - maximum speed from algorithm
+    double m_maxspeed_value;
+    double m_maxspeed_current;
+    double m_maxspeed_last;
+    double m_maxspeed_delta;
+    double m_maxspeed_avg;
+    std::deque<double> m_maxspeed_smooth;
+
+    // calculated data - driven distance
+    double m_distance_current;
+    double m_distance_last;
+    double m_distance_delta;
+
+    // calculated data - acceleration
+    double m_acceleration_current;
+    double m_acceleration_last;
+    double m_acceleration_min;
+    double m_acceleration_max;
+    std::deque<double> m_acceleration_mintime;
+    std::deque<double> m_acceleration_maxtime;
+    std::deque<double> m_acceleration_smooth;
+    double m_acceleration_delta;
+
+    // calculated data - turn (0..1)
+    double m_turn_current;
+    double m_turn_last;
+    double m_turn_delta;
+
+    // calculated data - angle derived from turn and MAX_STEERING_ANGLE
+    double m_angle_current;
+    double m_angle_last;
+    double m_angle_delta;
+    std::deque<double> m_angle_smooth;
+
+    // helper
+    std::string getLogPath();
+    std::string getTimeString();
+
+    // log writing
+    void writeLogFile(std::string delimiter, std::string filename);
+    void publishHud();
+
+    public:
+    LogStats();
+
+    private:
+    void controlledDriveParametersCallback(const drive_msgs::drive_param::ConstPtr& drive_parameters);
+    void maxSpeedCallback(const std_msgs::Float64::ConstPtr& max_speed);
+    // void gazeboStateTelemetryCallback(const drive_msgs::gazebo_state_telemetry::ConstPtr& gazebo_state_telemetry);
+};

ros_ws/src/analysis-tools/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<package format="2">
+  <name>analysis-tools</name>
+  <version>0.0.0</version>
+  <description>Tools for analysing the driving behaviour.</description>
+
+  <!-- Maintainer -->
+  <maintainer email="[email protected]">PG F1/10</maintainer>
+
+  <!-- License -->
+  <license>MIT</license>
+  <license>GPLv3</license>
+
+  <!-- Dependencies -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>car_config</depend>
+  <depend>vesc_sim</depend>
+  <depend>drive_msgs</depend>
+  <depend>geometric_math</depend>
+
+</package>