Add TerrainSimplification and TerrainSimplificationRos packages #5

oliwiermelon · 2021-01-17T18:48:53Z

Hi @benoit-robotics,
Can you please review the code of both TerrainSimplification and TerrainSimplificationRos packages.
Instructions on how to run the application are in the README.

benoit-robotics · 2021-01-19T10:33:12Z

I will review it later this week

…_seg into add-terrain-simplification

benoit-robotics · 2021-01-26T10:44:08Z

terrain_simplification_ros/src/terrain_simplification_ros.cpp

+
+bool
+TerrainSimplificationRos::readParameters() {
+  if (!ros_nh_.getParam("/terrain_simplification_ros_node/topic_elevation_map", topic_elevation_map_)){


getParam("topic_elevation_map") should work ? Same comment for all the getParam calls

I use ros::NodeHandle node_handle("~"); and the parameter is currently defined in the main .launch file. What method do you recommend? Overall, It is not the most useful as you cannot change the topic without restarting the node.

I think you don't need to specify the name of the node

ros_nh_.getParam("topic_elevation_map", topic_elevation_map_)

benoit-robotics · 2021-01-26T10:45:30Z

terrain_simplification_ros/src/terrain_simplification_ros.cpp

+double TerrainSimplificationRos::getTraversability(
+    const Eigen::Vector2d& location,
+    bool& is_inside) {
+  TerrainSimplification::getValueAtPosition("traversability", location, is_inside);


missing return statement ?

benoit-robotics · 2021-01-26T10:53:40Z

terrain_simplification/src/terrain_simplification.cpp

+  k = cv::Mat::zeros(k_size_, k_size_, CV_64F);
+  k.row((int)(k_size_-1)/2) = cv::getGaussianKernel(k_size_, -1, CV_64F).t();
+  double yaw = getRobotYaw();
+  cv::warpAffine(


weird formatting

I thought it is impossible to understand if it is in-line because of one of the inputs being cv::getRotationMatrix2D(). Should I:

keep it, or

have everything in a single line, or

put cv::getRotationMatrix2D()on a separate line, or

compute cv::getRotationMatrix2D() before cv::warpAffine()?

you can keep it, or put the arguments in two or three lines

benoit-robotics · 2021-01-26T10:59:11Z

terrain_simplification/src/terrain_simplification.cpp

+
+double
+TerrainSimplification::getRobotYaw() {
+  mutex_state_.lock();


Instead of locking and unlocking a mutex at the end of a method, you can use this : http://www.cplusplus.com/reference/mutex/lock_guard/

benoit-robotics · 2021-01-26T11:09:00Z

terrain_simplification/src/terrain_simplification.cpp

+
+TerrainSimplification::TerrainSimplification ()
+  : thread_loop_(true),
+    thread_run_(&TerrainSimplification::run, this) {


I am not sure I understand why you need to run the gridmap simplification in a new thread. Why don't you just simplify the gridmap in the main thread when a new gridmap is set ?

As the elevation map is very large (6 m x 6 m) but slow to update, this allows me to compute and publish a submap at any (achievable) rate. The main need arises as a result of the computation being dependent on the orientation of the robot. In addition, it gives me the ability to visualize it at a higher frequency (it is not limited by how long the filters take as I don't wait for them to be finished computing).

Use std::lock_guard() whenever possible. getSimplifiedGridMap() continues to use many lock() and unlock() because some functions, e.g. FilterChain-relatd, can take a long time

…d setRobotPose() in the ROS class

…errainSimplification instead of inheriting from it

…cesses

mauricefallon · 2021-02-02T16:15:11Z

@oliwiermelon : could you put some visualisations from group meetings into this PR.
People in the Cerberus team might be interested in this capability.

…e published map

… (e.g. derivatives)

…nt_cloud_odom to odom transformation Publish the simplified map in odom, with an offset to compensate for the drift

wxmerkt

I added a few comments inline however the biggest changes are architectural:

rosparams should be loaded in the launch file and not via a system call in the node
to do timing/cron-job like, I don't think a service call node is the right solution unless this is explicitly required for integration - having a parameter publish_frequency or process_every_n would do the job

I'll take the rest up via DM in Slack

wxmerkt · 2021-02-23T18:55:03Z

terrain_simplification/README.md

@@ -0,0 +1,7 @@
+![A gif demonstrating the use of terrain simplification as a base-motion-constraint, shown at 5x speed.](https://github.com/ori-drs/plane_seg/blob/add-terrain-simplification/terrain_simplification/terrain-simplification_base-motion-constraint_5x-speed.gif)


These links will no longer work after this branch has been merged and deleted

wxmerkt · 2021-02-23T18:56:05Z

terrain_simplification_ros/README.md

@@ -0,0 +1,30 @@
+# Description
+See https://wiki.oxfordrobots.com/display/~oliwier/2021/01/07/Terrain+Simplification


Internal link

wxmerkt · 2021-02-23T18:57:06Z

terrain_simplification_ros/src/terrain_simplification_ros_service_caller.cpp

@@ -0,0 +1,23 @@
+#include "ros/ros.h"


Use < and >

wxmerkt · 2021-02-23T18:57:38Z

terrain_simplification_ros/src/terrain_simplification_ros_service_caller.cpp

+  {
+    nh.param("/terrain_simplification_ros_service_caller/rate", rate, 10.);
+    ros_client_pub.call(req,res);
+    ros::Rate r(rate);


Pull this line out of the while

wxmerkt · 2021-02-23T18:58:25Z

terrain_simplification_ros/src/terrain_simplification_ros_service_caller.cpp

+  ros::ServiceClient ros_client_pub
+      = nh.serviceClient<std_srvs::Empty>("/terrain_simplification/pub");
+  std_srvs::Empty::Request req;
+  std_srvs::Empty::Response res;


Add a waitForExistence() - cf. https://docs.ros.org/en/noetic/api/roscpp/html/classros_1_1ServiceClient.html#a530e3f1d55cf50ab00b8e5bbd9e8230a

wxmerkt · 2021-02-23T19:02:04Z

terrain_simplification_ros/src/terrain_simplification_ros.cpp

+    sys_rtrn = system(cmd.c_str());
+  }
+  catch (std::exception& e) {
+    ROS_ERROR_STREAM("Wrong path to configuration (config) file!");


Why not concatenate e.what() to the message?

wxmerkt · 2021-02-23T19:09:17Z

terrain_simplification_ros/package.xml

+  <depend>grid_map_filters</depend>
+  <depend>terrain_simplification</depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>rqt_bag</exec_depend>


I understand why you have this here. Ideally, we'd like to avoid heavy UI dependencies listed in the package.xml unless necessary (imagine installing this on a robot and running rosdep install --from-paths ./ -iry - this will pull in all of Qt etc.)

wxmerkt · 2021-02-23T19:12:31Z

terrain_simplification_ros/src/terrain_simplification_ros.cpp

+TerrainSimplificationRos::loadConfigFile() {
+  try {
+    // load standard config
+    std::string cmd = "rosparam load " + ros::package::getPath("terrain_simplification_ros") + "/config/config.yaml";


This is odd (I have seen similar in original towr). Just use a rosparam load in the launch file instead?

wxmerkt · 2021-02-23T19:13:32Z

terrain_simplification/include/terrain_simplification/terrain_simplification.hpp

+
+  // Gridmaps' parameters
+  Eigen::Vector2d map_size_;                ///< size of the submap
+  int k_size_ = 51;                         ///< size of the kernel (for the directional Gaussian blur filter)


Possibly should be exposed as a rosparam since it may depend on the grid map resolution

wxmerkt · 2021-02-23T20:14:37Z

terrain_simplification_ros/README.md

+```
+These can be set and re-read using:
+```
+rosservice call /terrain_simplification/read


Do you use this actively? Some of the information read-in via readParameters wont be changeable during runtime... since you already set up subscriptions etc. (which could be remapped instead)

… launch file

…ceptions

Removes hard-coded assumptions about a 2cm elevation map grid resolution

…use with rhecall

…layer from map_simplified_wo_traversability

…rom to prevent the issue of the map being resized

…problems with addLayerFromImage()

…ilter_chain header TODO: remove filter chain from TerrainSimplification

…ters_drs for inpainting NOTE: pull changes in grid_map_filters_drs TODO: remove filter_chain from TerrainSimplification

odom to point cloud odom

wxmerkt · 2021-07-26T13:33:23Z

terrain_simplification_ros/package.xml

+  <depend>grid_map_core</depend>
+  <depend>grid_map_cv</depend>
+  <depend>grid_map_filters</depend>
+  <depend>grid_map_filters_drs</depend>


Note, this is a private repository. I understand this PR has been stale for a while and we need to clean it up at some point - but adding a private dependency for a public repository won't work. One way is to move the runtime config + launch file to the user repository and not use it by default in the config included here

I agree; I admit, I have not thought about it.

If we do what you suggest, can a filter from grid_map_filters_drs still be loaded by filter_chain even if the repository is not added as a dependency? If so, I am happy to include the specific config.yaml and .launch files in e.g. RHECALL.

To my knowledge, yes (the dependencies need to be at the level of the package that has the launch file / config), the point of using pluginlib is to be able to split packages and load plugins (here filters) dynamically

Okay, I will proceed as you suggested.

Something to bear in mind is that we will need to revisit the same problem when we make RHECALL publicly available.

Let's cross that bridge when we get to it :-)

…ilters_drs It's a runtime plugin, not a compile-time dependency

oliwiermelon and others added 2 commits January 17, 2021 18:30

Add TerrainSimplification and TerrainSimplificationRos packages

6660160

Create README.md

c93e508

oliwiermelon requested a review from benoit-robotics January 17, 2021 18:48

oliwiermelon assigned benoit-robotics Jan 17, 2021

oliwiermelon added 3 commits January 21, 2021 10:48

Check that the queried position is inside the map

b4c9391

Add ros server to get a value from a map at a position, create a srv

c7a61f4

Merge branch 'add-terrain-simplification' of github.com:ori-drs/plane…

e676deb

…_seg into add-terrain-simplification

benoit-robotics reviewed Jan 26, 2021

View reviewed changes

oliwiermelon added 11 commits January 26, 2021 14:39

[TerrainSimplification] change from boost::shared_mutex to std::mutex

55810e6

Use std::lock_guard() whenever possible. getSimplifiedGridMap() continues to use many lock() and unlock() because some functions, e.g. FilterChain-relatd, can take a long time

[TerrainSimplificationRos::getTraversability] add missing return keyword

f21eebc

[TerrainSimplificationRos] load parameters from a yaml config file

a8eff16

[TerrainSimplificationRos] add doxygen comments

38bffbc

[TerrainSimplification, TerrainSimplificationRos] use setGridMap() an…

4b6bd58

…d setRobotPose() in the ROS class

[TerrainSimplification, TerrainSimplificationRos] use shared_ptr to T…

a6c8887

…errainSimplification instead of inheriting from it

[TerrainSimplificationRos] add getTerrainSimplification()

91f2839

[TerrainSimplificationRos] fix ROS param name

772755f

[TerrainSimplificationRos] make getTerrainSimplification() public

5a24948

[TerrainSimplification] add catkin dependencies and link libraries

0fae537

[TerrainSimplification::getSimplifiedGridMap] Rearrange and group pro…

a4a5f56

…cesses

benoit-robotics and others added 5 commits February 3, 2021 16:44

remove lcm based executable

7ab7b59

Merge branch 'master' into add-terrain-simplification

61cf5fc

[TerrainSimplificationRos::pubSimplifiedMap] fix small x offset in th…

2fd6ade

…e published map

[terrain_simplification] add OpenCV to CMakeLists

422ca45

[TerrainSimplification] create containers (struct) for related images…

a5bca91

… (e.g. derivatives)

Oliwier Melon added 5 commits February 9, 2021 16:44

[TerrainSimplification, TerrainSimplificationRos] account for the poi…

a12b94c

…nt_cloud_odom to odom transformation Publish the simplified map in odom, with an offset to compensate for the drift

[terrain_simplification_ros] change trottled topic name

35b4c1b

[TerrainSimplification] fix minor bug

ab200cc

[terrain_simplification] clean CMakeLists.txt and package.xml

0f879e5

[terrain_simplification_ros] update README.md

ab459ba

wxmerkt reviewed Feb 23, 2021

View reviewed changes

wxmerkt and others added 18 commits February 24, 2021 11:33

[terrain_simplification] Fix virtual destructor

448eb58

[terrain_simplification_ros] Replace loadConfigFile with loading from…

9471693

… launch file

[terrain_simplification_ros] Replace "success" in constructor with ex…

2541404

…ceptions

[terrain_simplification] Make grid resolution configurable

b3bd970

Removes hard-coded assumptions about a 2cm elevation map grid resolution

[terrain_simplification] Typos & add TODO comments

c8c9b25

[terrain_simplification] Minor

e18c39d

[terrain_simplification_ros] comment out filters in filter_chain for …

318ee8f

…use with rhecall

[TerrainSimplification] make filter chain an option

608777b

[TerrainSimplification::simplifyGridMap] remove default "simplified" …

9b6e9b4

…layer from map_simplified_wo_traversability

[TerrainSimplification::simplifyGridMap] fix spelling

11a102f

[TerrainSimplification::simplifyGridMap] change arguments of addDataF…

9156a01

…rom to prevent the issue of the map being resized

[TerrainSimplification::convertCvImageToGridMap] check and print out …

7284ba0

…problems with addLayerFromImage()

[terrain_simplification_ros] load filter_chain within the ros node

147218f

[TerrainSimplificationRos] improve warnings

b746de5

[TerrainSimplification, TerrainSimplificationRos] update deprecated f…

2b1deb8

…ilter_chain header TODO: remove filter chain from TerrainSimplification

[terrain_simplification, terrain_simplification_ros] use grid_map_fil…

4d642ac

…ters_drs for inpainting NOTE: pull changes in grid_map_filters_drs TODO: remove filter_chain from TerrainSimplification

[TerrainSimplification] comment out derivatives

a372902

[terrain_simplification_ros] add fake tf publisher

a599266

odom to point cloud odom

wxmerkt reviewed Jul 26, 2021

View reviewed changes

oliwiermelon and others added 4 commits July 26, 2021 15:34

[ts, ts_ros] make filtered layer name settable

df88525

[terrain_simplification] Ignore warnings from filter_chain

25c688c

[terrain_simplification_ros] Remove explicit dependency on grid_map_f…

d1f2c28

…ilters_drs It's a runtime plugin, not a compile-time dependency

[terrain_simplification_ros] Update CMake version

a3efd44

wxmerkt mentioned this pull request Nov 1, 2021

plane_seg: ~15-20% performance improvement #13

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add TerrainSimplification and TerrainSimplificationRos packages #5

Add TerrainSimplification and TerrainSimplificationRos packages #5

oliwiermelon commented Jan 17, 2021 •

edited

Loading

benoit-robotics commented Jan 19, 2021

benoit-robotics Jan 26, 2021 •

edited

Loading

oliwiermelon Jan 26, 2021

benoit-robotics Jan 26, 2021

benoit-robotics Jan 26, 2021

benoit-robotics Jan 26, 2021

oliwiermelon Jan 26, 2021

benoit-robotics Jan 26, 2021

benoit-robotics Jan 26, 2021 •

edited

Loading

benoit-robotics Jan 26, 2021 •

edited

Loading

oliwiermelon Jan 26, 2021 •

edited

Loading

mauricefallon commented Feb 2, 2021

wxmerkt left a comment

wxmerkt Feb 23, 2021

wxmerkt Feb 23, 2021

wxmerkt Feb 23, 2021

wxmerkt Feb 23, 2021

wxmerkt Feb 23, 2021

wxmerkt Feb 23, 2021

wxmerkt Feb 23, 2021

wxmerkt Feb 23, 2021

wxmerkt Feb 23, 2021

wxmerkt Feb 23, 2021

wxmerkt Jul 26, 2021

oliwiermelon Jul 28, 2021

wxmerkt Jul 28, 2021

oliwiermelon Jul 28, 2021

wxmerkt Jul 28, 2021

		@@ -0,0 +1,7 @@
		![A gif demonstrating the use of terrain simplification as a base-motion-constraint, shown at 5x speed.](https://github.com/ori-drs/plane_seg/blob/add-terrain-simplification/terrain_simplification/terrain-simplification_base-motion-constraint_5x-speed.gif)

		@@ -0,0 +1,30 @@
		# Description
		See https://wiki.oxfordrobots.com/display/~oliwier/2021/01/07/Terrain+Simplification

Add TerrainSimplification and TerrainSimplificationRos packages #5

Are you sure you want to change the base?

Add TerrainSimplification and TerrainSimplificationRos packages #5

Conversation

oliwiermelon commented Jan 17, 2021 • edited Loading

benoit-robotics commented Jan 19, 2021

benoit-robotics Jan 26, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

benoit-robotics Jan 26, 2021 • edited Loading

Choose a reason for hiding this comment

benoit-robotics Jan 26, 2021 • edited Loading

Choose a reason for hiding this comment

oliwiermelon Jan 26, 2021 • edited Loading

Choose a reason for hiding this comment

mauricefallon commented Feb 2, 2021

wxmerkt left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

oliwiermelon commented Jan 17, 2021 •

edited

Loading

benoit-robotics Jan 26, 2021 •

edited

Loading

benoit-robotics Jan 26, 2021 •

edited

Loading

benoit-robotics Jan 26, 2021 •

edited

Loading

oliwiermelon Jan 26, 2021 •

edited

Loading