new message type for obstacles

uzh-rpg · Mar 8, 2022 · 4172483 · 4172483
1 parent 79ed168
commit 4172483
Show file tree

Hide file tree

Showing 7 changed files with 75 additions and 27 deletions.
diff --git a/envsim/include/envsim/visionsim.hpp b/envsim/include/envsim/visionsim.hpp
@@ -11,7 +11,6 @@
 
 #include <filesystem>
 
-#include "std_msgs/Float32MultiArray.h"
 #include "std_msgs/String.h"
 
 // -- agilicious
@@ -81,8 +80,6 @@ class VisionSim {
   std::unique_ptr<flightlib::VisionEnv> vision_env_ptr_;
   flightlib::FrameID frame_id_;
 
-  int num_detected_obstacles_;
-
   // -- Race tracks
   Vector<3> start_pos_;
   Vector<3> goal_pos_;

diff --git a/envsim/package.xml b/envsim/package.xml
@@ -1,24 +1,23 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>envsim</name>
-  <version>0.0.1</version>
-  <description>ROS-Wrapper for AgiLib</description>
+    <name>envsim</name>
+    <version>0.0.1</version>
+    <description>ROS-Wrapper for AgiLib</description>
 
-  <maintainer email="foehn@ifi.uzh.ch">foehnph</maintainer>
+    <maintainer email="song@ifi.uzh.ch">Yunlong Song</maintainer>
 
-  <license>GNU GPL</license>
+    <license>GNU GPL</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <buildtool_depend>catkin_simple</buildtool_depend>
+    <buildtool_depend>catkin</buildtool_depend>
+    <buildtool_depend>catkin_simple</buildtool_depend>
 
-  <depend>agilib</depend>
-  <depend>agiros</depend>
-  <depend>roscpp</depend>
-  <depend>rosgraph_msgs</depend>
-
-
-  <depend>cv_bridge</depend>
-  <depend>flightlib</depend>
-  <depend>image_transport</depend>
+    <depend>agilib</depend>
+    <depend>agiros</depend>
+    <depend>cv_bridge</depend>
+    <depend>envsim_msgs</depend>
+    <depend>flightlib</depend>
+    <depend>image_transport</depend>
+    <depend>roscpp</depend>
+    <depend>rosgraph_msgs</depend>
 
 </package>
diff --git a/envsim/src/visionsim_node.cpp b/envsim/src/visionsim_node.cpp
@@ -2,6 +2,7 @@
 
 #include "agiros_msgs/QuadState.h"
 #include "envsim/visionsim.hpp"
+#include "envsim_msgs/ObstacleArray.h"
 #include "nav_msgs/Odometry.h"
 #include "rosgraph_msgs/Clock.h"
 
@@ -22,7 +23,7 @@ VisionSim::VisionSim(const ros::NodeHandle &nh, const ros::NodeHandle &pnh)
   image_transport::ImageTransport it(pnh_);
 
   obstacle_pub_ =
-    pnh_.advertise<std_msgs::Float32MultiArray>("groundtruth/obstacles", 1);
+    pnh_.advertise<envsim_msgs::ObstacleArray>("groundtruth/obstacles", 1);
 
   image_pub_ = it.advertise("unity/image", 1);
   depth_pub_ = it.advertise("unity/depth", 1);
@@ -238,19 +239,25 @@ void VisionSim::publishObstacles(const QuadState &state) {
 
   vision_env_ptr_->getQuadrotor()->setState(unity_quad_state);
 
-  //
-  std_msgs::Float32MultiArray msg_obstacle_state;
-  num_detected_obstacles_ = vision_env_ptr_->getNumDetectedObstacles();
+  envsim_msgs::ObstacleArray obstacle_msg;
+  obstacle_msg.header.stamp = ros::Time(state.t);
+  obstacle_msg.t = state.t;
+  obstacle_msg.num = vision_env_ptr_->getNumDetectedObstacles();
 
   flightlib::Vector<> obstacle_state;
-  obstacle_state.resize(3 * num_detected_obstacles_);
+  obstacle_state.resize(4 * obstacle_msg.num);
   vision_env_ptr_->getObstacleState(obstacle_state);
 
+  for (int i = 0; i < obstacle_msg.num; i++) {
+    envsim_msgs::Obstacle single_obstacle;
+    single_obstacle.position.x = obstacle_state[4 * i];
+    single_obstacle.position.x = obstacle_state[4 * i + 1];
+    single_obstacle.position.x = obstacle_state[4 * i + 2];
+    single_obstacle.scale = obstacle_state[4 * i + 3];
 
-  for (int i = 0; i < obstacle_state.size(); i++) {
-    msg_obstacle_state.data.push_back(obstacle_state[i]);
+    obstacle_msg.obstacles.push_back(single_obstacle);
   }
-  obstacle_pub_.publish(msg_obstacle_state);
+  obstacle_pub_.publish(obstacle_msg);
 }
 
 

diff --git a/envsim_msgs/CMakeLists.txt b/envsim_msgs/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.0.0)
+project(envsim_msgs)
+
+add_compile_options(-std=c++17)
+add_compile_options(-O3)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+cs_install()
+cs_export()
diff --git a/envsim_msgs/msg/Obstacle.msg b/envsim_msgs/msg/Obstacle.msg
@@ -0,0 +1,5 @@
+# Position of obstacle
+geometry_msgs/Vector3 position
+
+# Scale of obstacle
+float64 scale
diff --git a/envsim_msgs/msg/ObstacleArray.msg b/envsim_msgs/msg/ObstacleArray.msg
@@ -0,0 +1,10 @@
+Header header 
+
+# Time [s]
+float64 t
+
+# Number of obstacles in message
+int64 num
+
+# Obstacle array
+envsim_msgs/Obstacle[] obstacles
diff --git a/envsim_msgs/package.xml b/envsim_msgs/package.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>envsim_msgs</name>
+  <version>0.0.0</version>
+  <description>The envsim_msgs package</description>
+
+  <maintainer email="[email protected]">Elia Kaufmann</maintainer>
+  <license>MIT</license>
+
+  <author>Elia Kaufmann</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>message_generation</depend>
+  <depend>std_msgs</depend>
+
+</package>