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

[jsk_topic_tools][LightweightThrottle] dynamic change update_rate #1514

Merged
merged 1 commit into from Jun 5, 2017

Conversation

Projects
None yet
3 participants
@furushchev
Copy link
Contributor

commented Apr 9, 2017

support changing update_rate using dynamic_reconfigure

@furushchev furushchev force-pushed the furushchev:dynamic-throttle branch from 1b44f97 to 027dd9b Apr 9, 2017

geometry_msgs
image_transport
message_generation
nodelet tf

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 21, 2017

Member

please add newline

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 22, 2017

Author Contributor

Sorry, fixed.

message_generation
nodelet tf
roscpp
rostime

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 21, 2017

Member

is this needed?

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 22, 2017

Author Contributor

which one?

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 22, 2017

Author Contributor

rostime

I don't know. I didn't add this.

rostime
std_msgs
std_srvs
topic_tools

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 21, 2017

Member

duplicate?

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 22, 2017

Author Contributor

Right. Fixed.


gen = ParameterGenerator()

gen.add("update_rate", double_t, 0, "", 1.0, 0.0, 1000.0)

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 21, 2017

Member

Please add description.

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 22, 2017

Author Contributor

Oops, added.

pnh_.param("update_rate", update_rate_, 1.0); // default 1.0

srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(pnh_);
typename dynamic_reconfigure::Server<Config>::CallbackType f =

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 21, 2017

Member

typename is necessary?

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 22, 2017

Author Contributor

Right. Removed.

@@ -95,12 +106,13 @@ namespace jsk_topic_tools
pub_ = pnh_.advertise(opts);
}
ros::Time now = ros::Time::now();
if ((now - latest_stamp_).toSec() > 1 / update_rate_) {
if (update_rate_ > 0.0 && (now - latest_stamp_).toSec() > 1.0 / update_rate_) {

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 21, 2017

Member

1.0 can be 1 because update_rate_ is double, right?

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 21, 2017

Member

0.0 can be 0 right? (I prefer simpler way)

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 22, 2017

Author Contributor

Why we must convert type implicitly?
For me update_rate_ is double and 0.0 is also double, so it looks fine.

void LightweightThrottle::configCallback(Config& config, uint32_t level)
{
boost::mutex::scoped_lock lock(mutex_);
update_rate_ = config.update_rate;

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 21, 2017

Member

Why not show warning message if update_rate <= 0?

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 22, 2017

Author Contributor

I want to use update_rate == 0 as stopping message publishing.

@@ -24,6 +31,9 @@ add_message_files(
add_service_files(
FILES List.srv Update.srv ChangeTopic.srv PassthroughDuration.srv
)
generate_dynamic_reconfigure_options(
cfg/LightweightThrottle.cfg
)

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 21, 2017

Member

You need to add

add_dependencies(jsk_topic_tools ${PROJECT_NAME}_gencfg)

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 22, 2017

Author Contributor

Added.

@wkentaro wkentaro self-assigned this Apr 21, 2017

@wkentaro

This comment has been minimized.

Copy link
Member

commented Apr 22, 2017

@wkentaro

This comment has been minimized.

Copy link
Member

commented Apr 23, 2017

@furushchev BTW, this node use latest_stamp_ and it looks that it does not work with rosbag play --loop, could you please fix it by seeing /use_sim_time or monitoring time series?

@furushchev furushchev force-pushed the furushchev:dynamic-throttle branch from 473a2bc to eb1cdc7 Apr 23, 2017

@furushchev

This comment has been minimized.

Copy link
Contributor Author

commented Apr 23, 2017

@wkentaro Done. 👍

if ((now - latest_stamp_).toSec() > 1 / update_rate_) {

if (latest_stamp_ > now) {
ROS_WARN("Detected jump back in time.");

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 23, 2017

Member

Maybe ROS_ERROR is better.

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 23, 2017

Member

Please describe that the latest_stamp_ will be overwritten.

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 23, 2017

Author Contributor

I think it is already enough.
ros/geometry2@619a132

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 23, 2017

Member

Why?
it says Detected jump back in time. Clearing TF buffer. and You should say Overwriting latest_stamp. I think.

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 23, 2017

Author Contributor

Because we can clearly recognize that time is jumped back and I think there is no one who want to know the fact that 'Overwriting latest_stamp', because most of users should not be conscious about latest_stamp_ variable and core developers can know immediately this simple mechanism.

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 23, 2017

Member

Hmm, I disagree.
Because

  • with Detected jump back in time, I expect it does not change behavior. but it actually overwrites the last published time and it changes the publishing rate at that time.
  • This node is for core developers IMO, and I'm not sure why you need to close the detailed behavior.

users should not be conscious about latest_stamp_ variable

  • You can add better message, for example Detected jump back in time. Clearing time buffer of latest message publication (for throttling).

@k-okada What do you think?

This comment has been minimized.

Copy link
@k-okada

k-okada Apr 24, 2017

Member

ok, let's split this PR into dynamic change update_rate and jump back in time
and question is

  1. when jump back in time occurs, only is simulation or does real robot will set back time time?
  2. when we need lightweight throttle, other than DRC? can we run all node within in the robot network, or do we have better wifi connection?

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 24, 2017

Author Contributor

@k-okada Sorry, I didn't notice.

  1. when jump back in time occurs, only is simulation or does real robot will set back time time?

I believe time jumps only on simulation or playing rosbag.

  1. when we need lightweight throttle, other than DRC? can we run all node within in the robot network, or do we have better wifi connection?

I want this feature to throttle CPU resource for recognition pipeline according to recognition reliability.

ros::Time now = ros::Time::now();
if ((now - latest_stamp_).toSec() > 1 / update_rate_) {

if (latest_stamp_ > now) {

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 23, 2017

Member

Please check if /use_sim_time is true.

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 23, 2017

Member

with using unixtime, IMO the latest_stamp_ should not be overwritten.
What do you think?

This comment has been minimized.

Copy link
@furushchev

furushchev Apr 23, 2017

Author Contributor

You mean wall time? If so, wall time does not jump back. so don't worry.

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 23, 2017

Member

I see that latest_stamp_ and now are got by ros::Time::now not from header.
Looks good.

if ((now - latest_stamp_).toSec() > 1 / update_rate_) {

if (latest_stamp_ > now) {
ROS_WARN("Detected jump back in time.");

This comment has been minimized.

Copy link
@wkentaro

wkentaro Apr 23, 2017

Member

Hmm, I disagree.
Because

  • with Detected jump back in time, I expect it does not change behavior. but it actually overwrites the last published time and it changes the publishing rate at that time.
  • This node is for core developers IMO, and I'm not sure why you need to close the detailed behavior.

users should not be conscious about latest_stamp_ variable

  • You can add better message, for example Detected jump back in time. Clearing time buffer of latest message publication (for throttling).

@k-okada What do you think?

@furushchev furushchev force-pushed the furushchev:dynamic-throttle branch from eb1cdc7 to 466cd5a Jun 1, 2017

[jsk_topic_tools][LightweightThrottle] dynamic change update_rate
[jsk_topic_tools][lightweight_throttle] support jump back in time

@furushchev furushchev force-pushed the furushchev:dynamic-throttle branch from 466cd5a to 140d31e Jun 1, 2017

@furushchev

This comment has been minimized.

Copy link
Contributor Author

commented Jun 1, 2017

Sorry for very late response (I forgot to push the commit)
Added warning message to overwrite latest_stamp_.

@k-okada k-okada merged commit 5131bac into jsk-ros-pkg:master Jun 5, 2017

1 check passed

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

@furushchev furushchev deleted the furushchev:dynamic-throttle branch Jun 5, 2017

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.