From 79a89b4e1a6c4b1bd669e1a71eb22f6f571e1274 Mon Sep 17 00:00:00 2001 From: telemething Date: Tue, 12 Oct 2021 17:15:06 -0700 Subject: [PATCH 1/4] Removed retro_log --- Ros/src/retro_log/CMakeLists.txt | 33 ------------ Ros/src/retro_log/README.md | 3 -- Ros/src/retro_log/launch/retro_log_launch.py | 24 --------- Ros/src/retro_log/package.xml | 20 ------- Ros/src/retro_log/src/retro_log.cpp | 55 -------------------- 5 files changed, 135 deletions(-) delete mode 100644 Ros/src/retro_log/CMakeLists.txt delete mode 100644 Ros/src/retro_log/README.md delete mode 100644 Ros/src/retro_log/launch/retro_log_launch.py delete mode 100644 Ros/src/retro_log/package.xml delete mode 100644 Ros/src/retro_log/src/retro_log.cpp diff --git a/Ros/src/retro_log/CMakeLists.txt b/Ros/src/retro_log/CMakeLists.txt deleted file mode 100644 index 8e9eba3..0000000 --- a/Ros/src/retro_log/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(retro_log) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-c -Wall -Wextra -Wpedantic -ggdb) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) - -#-------------------------------------------------- - -install(TARGETS -danger_zone - DESTINATION lib/${PROJECT_NAME}) - -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/Ros/src/retro_log/README.md b/Ros/src/retro_log/README.md deleted file mode 100644 index 7b91a61..0000000 --- a/Ros/src/retro_log/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# retro_log - -store ros bags starting at given time in the past diff --git a/Ros/src/retro_log/launch/retro_log_launch.py b/Ros/src/retro_log/launch/retro_log_launch.py deleted file mode 100644 index 07cce22..0000000 --- a/Ros/src/retro_log/launch/retro_log_launch.py +++ /dev/null @@ -1,24 +0,0 @@ -import os -import yaml -import launch - -from launch import LaunchDescription -from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - -PKG = "retro_log" - -def generate_launch_description(): - - recordbag = launch.actions.ExecuteProcess( - cmd=['ros2', 'bag', 'record', 'max-bag-size', '1000000' 'all'], - output='screen' - ) - - return LaunchDescription([recordbag]) - - - - diff --git a/Ros/src/retro_log/package.xml b/Ros/src/retro_log/package.xml deleted file mode 100644 index 3a3700f..0000000 --- a/Ros/src/retro_log/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - retro_log - 0.0.0 - Store ros bags starting at given time in the past - mark - TODO: License declaration - - ament_cmake - - std_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/Ros/src/retro_log/src/retro_log.cpp b/Ros/src/retro_log/src/retro_log.cpp deleted file mode 100644 index 2de61ed..0000000 --- a/Ros/src/retro_log/src/retro_log.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using std::placeholders::_1; - -template inline void unused(Args&&...) {} - -class SubscriberNode : public rclcpp::Node -{ -public: - - //***************************************************************************** - //* - //* - //* - //***************************************************************************** - - SubscriberNode(): Node("retro_log") - { - m_command_subscription = this->create_subscription( - m_command_topic_name, 10, std::bind(&SubscriberNode::command_callback, this, _1)); - } - -private: - - const std::string m_command_topic_name = "/retrolog/command"; - rclcpp::Subscription::SharedPtr m_command_subscription; - - //***************************************************************************** - //* - //* - //* - //***************************************************************************** - - void command_callback(const retro_log::msg::Command msg) const - { - } -}; - -//***************************************************************************** -//* -//* -//* -//***************************************************************************** - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} From 7f2c7d7cb430c970d994e0dbcf50a5a813d75f39 Mon Sep 17 00:00:00 2001 From: telemething Date: Tue, 12 Oct 2021 17:17:26 -0700 Subject: [PATCH 2/4] Removed the retro log dir --- Ros/src/retro_log/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 Ros/src/retro_log/COLCON_IGNORE diff --git a/Ros/src/retro_log/COLCON_IGNORE b/Ros/src/retro_log/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 From 2fbfa0e60960b4a0d7831f53b90974f366f15e88 Mon Sep 17 00:00:00 2001 From: telemething Date: Wed, 13 Oct 2021 11:34:12 -0700 Subject: [PATCH 3/4] Fixed texture leak in sim --- .../Assets/Scripts/Sensors/ImagePublisher.cs | 21 +++++++------------ 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/KomatsuSimulator/Assets/Scripts/Sensors/ImagePublisher.cs b/KomatsuSimulator/Assets/Scripts/Sensors/ImagePublisher.cs index a031ac1..6758d09 100644 --- a/KomatsuSimulator/Assets/Scripts/Sensors/ImagePublisher.cs +++ b/KomatsuSimulator/Assets/Scripts/Sensors/ImagePublisher.cs @@ -16,6 +16,7 @@ public class ImagePublisher : MonoBehaviour //private MessageTypes.Sensor.CompressedImage message; private Texture2D texture2D; + private Texture2D texture2DFlipped; private Rect rect; private const int isBigEndian = 1; private const int step = 3; @@ -27,7 +28,7 @@ public class ImagePublisher : MonoBehaviour /// Arg original: incoming texture /// Returns: void //************************************************************************* - public static void FlipTextureVerticallyInplace(Texture2D original) + public void FlipTextureVerticallyInplace(Texture2D original) { var originalPixels = original.GetPixels(); @@ -53,25 +54,18 @@ public static void FlipTextureVerticallyInplace(Texture2D original) /// Arg original: incoming texture /// Returns: Flipped texture //************************************************************************* - public static Texture2D FlipTextureVertically(Texture2D original) + public Texture2D FlipTextureVertically(Texture2D original) { - Texture2D flipped = new Texture2D( - original.width, original.height, original.format, false); - int xN = original.width; int yN = original.height; for (int i = 0; i < xN; i++) - { for (int j = 0; j < yN; j++) - { - flipped.SetPixel(i, yN - j - 1, original.GetPixel(i, j)); - } - } + texture2DFlipped.SetPixel(i, yN - j - 1, original.GetPixel(i, j)); - flipped.Apply(); + texture2DFlipped.Apply(); - return flipped; + return texture2DFlipped; } //************************************************************************ @@ -105,6 +99,7 @@ private void InitializeGameObject() { ImageCamera.enabled = true; texture2D = new Texture2D(resolutionWidth, resolutionHeight, TextureFormat.RGB24, false); + texture2DFlipped = new Texture2D(resolutionWidth, resolutionHeight, TextureFormat.RGB24, false); rect = new Rect(0, 0, resolutionWidth, resolutionHeight); ImageCamera.targetTexture = new RenderTexture(resolutionWidth, resolutionHeight, 24); } @@ -121,7 +116,7 @@ private void InitializeMessage() //************************************************************************ /// Description: Update message, send to ROS - /// Returns: void + /// Returns: void //************************************************************************* public IEnumerator UpdateMessage() { From d1eb4f54af7e5ee3b9a26085dcb72bb5f25b9476 Mon Sep 17 00:00:00 2001 From: telemething Date: Wed, 13 Oct 2021 13:25:26 -0700 Subject: [PATCH 4/4] Remove truck controls, set obj id and class id --- .../Assets/Scenes/Scenario2.unity | 102 ++++++++++-------- .../Sensors/DetectorGroundTruthSensor.cs | 13 ++- 2 files changed, 67 insertions(+), 48 deletions(-) diff --git a/KomatsuSimulator/Assets/Scenes/Scenario2.unity b/KomatsuSimulator/Assets/Scenes/Scenario2.unity index 2f0a66b..da00d28 100644 --- a/KomatsuSimulator/Assets/Scenes/Scenario2.unity +++ b/KomatsuSimulator/Assets/Scenes/Scenario2.unity @@ -1478,27 +1478,27 @@ PrefabInstance: objectReference: {fileID: 0} - target: {fileID: 6100550845703879732, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550845703879732, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMin.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550845703879732, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.x - value: 0 + value: 160 objectReference: {fileID: 0} - target: {fileID: 6100550845703879732, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.y - value: 0 + value: 31.504831 objectReference: {fileID: 0} - target: {fileID: 6100550845703879732, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.x - value: 0 + value: 10 objectReference: {fileID: 0} - target: {fileID: 6100550845703879732, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.y - value: 0 + value: -10 objectReference: {fileID: 0} - target: {fileID: 6100550846288094405, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.x @@ -1506,27 +1506,27 @@ PrefabInstance: objectReference: {fileID: 0} - target: {fileID: 6100550846288094405, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846288094405, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMin.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846288094405, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.x - value: 0 + value: 140 objectReference: {fileID: 0} - target: {fileID: 6100550846288094405, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.y - value: 0 + value: 26.50483 objectReference: {fileID: 0} - target: {fileID: 6100550846288094405, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.x - value: 0 + value: 10 objectReference: {fileID: 0} - target: {fileID: 6100550846288094405, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.y - value: 0 + value: -129.5145 objectReference: {fileID: 0} - target: {fileID: 6100550846340665528, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.x @@ -1534,52 +1534,60 @@ PrefabInstance: objectReference: {fileID: 0} - target: {fileID: 6100550846340665528, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846340665528, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMin.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846340665528, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.x - value: 0 + value: 160 objectReference: {fileID: 0} - target: {fileID: 6100550846340665528, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.y - value: 0 + value: 166.01932 objectReference: {fileID: 0} - target: {fileID: 6100550846340665528, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.x - value: 0 + value: 10 objectReference: {fileID: 0} - target: {fileID: 6100550846340665528, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.y + value: -93.00966 + objectReference: {fileID: 0} + - target: {fileID: 6100550846391146277, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} + propertyPath: m_IsActive value: 0 objectReference: {fileID: 0} - target: {fileID: 6100550846391146282, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846391146282, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMin.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846391146282, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.y - value: 0 + value: 269.029 objectReference: {fileID: 0} - target: {fileID: 6100550846391146282, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.x - value: 0 + value: 10 objectReference: {fileID: 0} - target: {fileID: 6100550846391146282, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.y - value: 0 + value: -10 objectReference: {fileID: 0} - target: {fileID: 6100550846582550105, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_Name value: UI objectReference: {fileID: 0} + - target: {fileID: 6100550846582550105, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} + propertyPath: m_IsActive + value: 1 + objectReference: {fileID: 0} - target: {fileID: 6100550846582550109, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_Pivot.x value: 0 @@ -1690,27 +1698,27 @@ PrefabInstance: objectReference: {fileID: 0} - target: {fileID: 6100550846614828753, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846614828753, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMin.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846614828753, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.x - value: 0 + value: 140 objectReference: {fileID: 0} - target: {fileID: 6100550846614828753, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.y - value: 0 + value: 26.50483 objectReference: {fileID: 0} - target: {fileID: 6100550846614828753, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.x - value: 0 + value: 10 objectReference: {fileID: 0} - target: {fileID: 6100550846614828753, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.y - value: 0 + value: -10 objectReference: {fileID: 0} - target: {fileID: 6100550846872089348, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.x @@ -1718,27 +1726,27 @@ PrefabInstance: objectReference: {fileID: 0} - target: {fileID: 6100550846872089348, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846872089348, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMin.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550846872089348, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.x - value: 0 + value: 160 objectReference: {fileID: 0} - target: {fileID: 6100550846872089348, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.y - value: 0 + value: 31.504831 objectReference: {fileID: 0} - target: {fileID: 6100550846872089348, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.x - value: 0 + value: 10 objectReference: {fileID: 0} - target: {fileID: 6100550846872089348, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.y - value: 0 + value: -51.50483 objectReference: {fileID: 0} - target: {fileID: 6100550847257495159, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.x @@ -1746,27 +1754,27 @@ PrefabInstance: objectReference: {fileID: 0} - target: {fileID: 6100550847257495159, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550847257495159, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMin.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550847257495159, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.x - value: 0 + value: 140 objectReference: {fileID: 0} - target: {fileID: 6100550847257495159, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.y - value: 0 + value: 31.50483 objectReference: {fileID: 0} - target: {fileID: 6100550847257495159, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.x - value: 0 + value: 10 objectReference: {fileID: 0} - target: {fileID: 6100550847257495159, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.y - value: 0 + value: -88.00966 objectReference: {fileID: 0} - target: {fileID: 6100550847513206136, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.x @@ -1774,27 +1782,27 @@ PrefabInstance: objectReference: {fileID: 0} - target: {fileID: 6100550847513206136, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550847513206136, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMin.y - value: 0 + value: 1 objectReference: {fileID: 0} - target: {fileID: 6100550847513206136, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.x - value: 0 + value: 140 objectReference: {fileID: 0} - target: {fileID: 6100550847513206136, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_SizeDelta.y - value: 0 + value: 31.50483 objectReference: {fileID: 0} - target: {fileID: 6100550847513206136, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.x - value: 0 + value: 10 objectReference: {fileID: 0} - target: {fileID: 6100550847513206136, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchoredPosition.y - value: 0 + value: -46.50483 objectReference: {fileID: 0} - target: {fileID: 6543429262768960798, guid: ca81b5e5df6f29051a9653b63cba8147, type: 3} propertyPath: m_AnchorMax.y @@ -3526,7 +3534,7 @@ MonoBehaviour: ScanAngleStartDegrees: -90 ScanAngleEndDegrees: 90 ScanOffsetAfterPublish: 0 - NumMeasurementsPerScan: 180 + NumMeasurementsPerScan: 90 TimeBetweenMeasurementsSeconds: 0 markerPrefab: {fileID: 7151730531324999659, guid: 32e2a897b3516fe70a4665a0b7d92eb2, type: 3} LayerMaskName: Shovel diff --git a/KomatsuSimulator/Assets/Scripts/Sensors/DetectorGroundTruthSensor.cs b/KomatsuSimulator/Assets/Scripts/Sensors/DetectorGroundTruthSensor.cs index e38e322..800ba95 100644 --- a/KomatsuSimulator/Assets/Scripts/Sensors/DetectorGroundTruthSensor.cs +++ b/KomatsuSimulator/Assets/Scripts/Sensors/DetectorGroundTruthSensor.cs @@ -41,6 +41,16 @@ private RosMessageTypes.BuiltinInterfaces.TimeMsg GetRosTime() return new RosMessageTypes.BuiltinInterfaces.TimeMsg((int)span.TotalSeconds, (uint)(span.Ticks * 100)); } + private string RemoveParenNumber(string nameIn) + { + var li = nameIn.LastIndexOf('('); + + if(0 > li) + return nameIn; + + return nameIn.Substring(0,li).Trim(); + } + public RosMessageTypes.Vision.Detection3DArrayMsg FindAllObjects(string tagName) { RosMessageTypes.Vision.Detection3DArrayMsg detectedObjectArray = @@ -111,11 +121,12 @@ public RosMessageTypes.Vision.Detection3DArrayMsg FindAllObjects(string tagName) detectedObjectArray.detections[index].header = new RosMessageTypes.Std.HeaderMsg(rosTime, rosFrame); detectedObjectArray.detections[index].bbox = new RosMessageTypes.Vision.BoundingBox3DMsg(center, size); detectedObjectArray.detections[index].results = new RosMessageTypes.Vision.ObjectHypothesisWithPoseMsg[1]; + detectedObjectArray.detections[index].id = go.name; //*** TODO : game object name is not correct here. We want an object type, like 'person' or 'truck'. Maybe we //*** can use a tag for this. - var hyp = new RosMessageTypes.Vision.ObjectHypothesisMsg(go.name, 1.0); + var hyp = new RosMessageTypes.Vision.ObjectHypothesisMsg(RemoveParenNumber(go.name), 1.0); var pwc = new RosMessageTypes.Geometry.PoseWithCovarianceMsg(center, covariance); detectedObjectArray.detections[index].results[0] =