Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding Max update rate parameter for octomap updaters #920

Merged

Conversation

Projects
None yet
4 participants
@Ridhwanluthra
Copy link
Contributor

commented May 26, 2018

Rapid updation of octomap was cousing various locking issues where the other nodes were not able to update the planning scene.

The callback is retured without excecution if the duration for maintaining the update rate is not maintained.

Checklist

  • Required: Code is auto formatted using clang-format
  • Extended the tutorials / documentation, if necessary reference
  • Include a screenshot if changing a GUI
  • Optional: Created tests, which fail without this PR reference
  • Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)
@v4hn
Copy link
Member

left a comment

Cross-reference: The corresponding tutorial change is proposed in https://github.com/PickNikRobotics/moveit_tutorials/pull/68 ros-planning/moveit_tutorials#177

@@ -78,6 +78,8 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater
image_transport::CameraPublisher pub_filtered_depth_image_;
image_transport::CameraPublisher pub_filtered_label_image_;

ros::WallTime last_update_time_;

This comment has been minimized.

Copy link
@v4hn

v4hn May 30, 2018

Member

If we use WallTime here, the behavior between simulation and real robot will differ (WallTime is independent of the actual realtime factor).
I would propose to use ros::Time instead.

This comment has been minimized.

Copy link
@Ridhwanluthra

Ridhwanluthra Jun 3, 2018

Author Contributor

done

This comment has been minimized.

Copy link
@v4hn

v4hn Jun 4, 2018

Member

You still left WallTime right above this comment.

This comment has been minimized.

Copy link
@mlautman

mlautman Jun 4, 2018

Member

Should we do a findreplace all ros::WallTime to ros::Time in these files or am I missing something?

This comment has been minimized.

Copy link
@v4hn

v4hn Jun 5, 2018

Member

No, the remaining WallTime I see here is for profiling the runtime of the update code and we want computation time there.

@@ -93,6 +93,7 @@ bool DepthImageOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)
readXmlParam(params, "shadow_threshold", &shadow_threshold_);
readXmlParam(params, "padding_scale", &padding_scale_);
readXmlParam(params, "padding_offset", &padding_offset_);
readXmlParam(params, "max_update_rate", &max_update_rate_);

This comment has been minimized.

Copy link
@v4hn

v4hn May 30, 2018

Member

This parameter should be optional in order not to break current setups.

The default value (probably 0?) should skip the check below and behave as before.

This comment has been minimized.

Copy link
@Ridhwanluthra

Ridhwanluthra Jun 3, 2018

Author Contributor

done, made it skip check for <= 0

This comment has been minimized.

Copy link
@mlautman

mlautman Jun 4, 2018

Member

Is that really the best default? Why not 10hz? It sounds like an unrestricted update rate is unusable.

This comment has been minimized.

Copy link
@v4hn

v4hn Jun 5, 2018

Member

Unrestricted is the default until now and if users throttle/control the input on their own (best practice), a different default can affect/break their setup.
If you want it changed, we could do that in melodic and document it in the MIGRATION file, but I would just leave it as is.
The tutorial should not even mention it's optional, so nobody should run in that problem again.

Ridhwanluthra added a commit to Ridhwanluthra/moveit_tutorials that referenced this pull request Jun 3, 2018

@v4hn
Copy link
Member

left a comment

format checking failed

@@ -78,6 +78,8 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater
image_transport::CameraPublisher pub_filtered_depth_image_;
image_transport::CameraPublisher pub_filtered_label_image_;

ros::WallTime last_update_time_;

This comment has been minimized.

Copy link
@v4hn

v4hn Jun 4, 2018

Member

You still left WallTime right above this comment.

Ridhwanluthra added some commits Jun 4, 2018

@@ -87,6 +89,8 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater
double shadow_threshold_;
double padding_scale_;
double padding_offset_;
// setting default 0 in order to ignore param
double max_update_rate_ = 0;

This comment has been minimized.

Copy link
@v4hn

v4hn Jun 5, 2018

Member

I only saw this now, sorry.
Please move the initialization to the cpp files, where everything else is initialized too. (for both updaters)

This comment has been minimized.

Copy link
@Ridhwanluthra

Ridhwanluthra Jun 6, 2018

Author Contributor

I did this because all of the other parameters were also initialized in the header file.
Why are we shifting this to cpp?

This comment has been minimized.

Copy link
@v4hn

v4hn Jun 6, 2018

Member

😕 I'm talking about the = 0, right?
All parameters are initialized with default values in the cpp as I see it and you specified the default value in the header for no reason.

The parameter should of course be defined in the header.

/* params */
std::string point_cloud_topic_;
double scale_;
double padding_;
double max_range_;
unsigned int point_subsample_;
// setting default 0 in order to ignore param
double max_update_rate_ = 0;

This comment has been minimized.

Copy link
@v4hn

v4hn Jun 5, 2018

Member

See above.

@v4hn v4hn force-pushed the Ridhwanluthra:max_update_rate_octomap branch from 71fa485 to 220295d Jun 11, 2018

@Ridhwanluthra

This comment has been minimized.

Copy link
Contributor Author

commented Jun 11, 2018

@v4hn if everything is ok can we merge this?

@v4hn

v4hn approved these changes Jun 11, 2018

@v4hn

This comment has been minimized.

Copy link
Member

commented Jun 11, 2018

I can't merge it, but I approved, so the next maintainer who has a moment to review this (@mlautman?) can merge this and ros-planning/moveit_tutorials#177 (review) .

Please also cherry-pick to melodic-devel of course.

@mlautman

This comment has been minimized.

Copy link
Member

commented Jun 11, 2018

@v4hn I don't have permissions yet to merge this in

@v4hn

This comment has been minimized.

Copy link
Member

commented Jun 11, 2018

I don't have permissions yet to merge this in

Then please get them from @davetcoleman , I don't have the permissions to add you here - although I would be quite happy if I could.

@v4hn v4hn merged commit a989aa4 into ros-planning:kinetic-devel Jun 11, 2018

1 check passed

continuous-integration/travis-ci/pr The Travis CI build passed
Details

v4hn added a commit that referenced this pull request Jun 11, 2018

Adding Max update rate parameter for octomap updaters (#920)
* adding the max update rate parameter to the point cloud octomap updater

* adding the max update rate parameter to the depth image octomap updater

* made max_update_rate parameter optional

* walltime to time
@v4hn

This comment has been minimized.

Copy link
Member

commented Jun 11, 2018

cherry-picked to melodic.

@skohlbr

This comment has been minimized.

Copy link
Contributor

commented Jun 17, 2018

Is there any particular reason for adding this now (as in: A bottleneck with the updaters discovered lately)? I'm asking because after switching out sensors on a robot setup, it seems that the PointCloudOctomap Updater (16.04 Kinetic from .debs) runs extremely slow. Unfortunately, I don't have comprehensive test data for comparison, but it feels like there might be some sort of regression that makes octomap updates take very long time.

@v4hn

This comment has been minimized.

Copy link
Member

commented Jun 18, 2018

@Ridhwanluthra Ridhwanluthra deleted the Ridhwanluthra:max_update_rate_octomap branch Jun 27, 2018

dg-shadow added a commit to shadow-robot/moveit that referenced this pull request Jul 30, 2018

Adding Max update rate parameter for octomap updaters (ros-planning#920)
* adding the max update rate parameter to the point cloud octomap updater

* adding the max update rate parameter to the depth image octomap updater

* made max_update_rate parameter optional

* walltime to time

ipa-hsd added a commit to ipa-hsd/moveit that referenced this pull request Mar 18, 2019

Adding Max update rate parameter for octomap updaters (ros-planning#920)
* adding the max update rate parameter to the point cloud octomap updater

* adding the max update rate parameter to the depth image octomap updater

* made max_update_rate parameter optional

* walltime to time
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.