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.

Sign up for GitHub

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

ROS1 recent feature port #613

Merged
merged 11 commits into from
Mar 22, 2021
Merged

ROS1 recent feature port #613

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
de19931
navsat_transform diagram to address #550 (#570)
mabelzhang Jun 5, 2020
07ee6c2
Fix frame id of imu in differential mode, closes #482
ayrton04 Dec 4, 2020
c954475
Added local Cartesian option to navsat_transform
ayrton04 Dec 4, 2020
b4017d2
Fix navsat_transform issue with starting on uneven terrain
ayrton04 Dec 4, 2020
b079b73
Fix typo in navsat_transform_node.rst (#588)
togaen Sep 4, 2020
08d7fca
Fix sign error in dFY_dP in transfer function jacobian
ayrton04 Dec 4, 2020
fe2a8b0
Making repeated state publication optional (#595)
ayrton04 Dec 11, 2020
0009b57
PR feedback
ayrton04 Dec 17, 2020
a9b7fb2
Fixing build issues
ayrton04 Jan 8, 2021
792c6c6
Fixing stamp issues
ayrton04 Feb 4, 2021
c127759
Linting and uncrustifying
ayrton04 Feb 4, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 7 additions & 4 deletions src/navsat_transform.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -614,13 +614,16 @@ void NavSatTransform::gpsFixCallback(
setTransformGps(msg);
}

double cartesianX = 0;
double cartesianY = 0;
double cartesian_x = 0;
double cartesian_y = 0;
std::string cartesian_zone_tmp;
navsat_conversions::LLtoUTM(
msg->latitude, msg->longitude, cartesianY, cartesianX,
msg->latitude,
msg->longitude,
cartesian_y,
cartesian_x,
cartesian_zone_tmp);
latest_cartesian_pose_.setOrigin(tf2::Vector3(cartesianX, cartesianY, msg->altitude));
latest_cartesian_pose_.setOrigin(tf2::Vector3(cartesian_x, cartesian_y, msg->altitude));
latest_cartesian_covariance_.setZero();

// Copy the measurement's covariance matrix so that we can rotate it later
Expand Down
16 changes: 9 additions & 7 deletions src/ros_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,8 @@ RosFilter<T>::RosFilter(const rclcpp::NodeOptions & options)
gravitational_acceleration_(9.80665),
history_length_(0),
latest_control_(),
last_diag_time_(0, 0, RCL_ROS_TIME),
last_published_stamp_(0, 0, RCL_ROS_TIME),
last_set_pose_time_(0, 0, RCL_ROS_TIME),
latest_control_time_(0, 0, RCL_ROS_TIME),
tf_timeout_(0),
Expand Down Expand Up @@ -133,7 +135,10 @@ void RosFilter<T>::reset()

// Also set the last set pose time, so we ignore all messages
// that occur before it
last_set_pose_time_ = rclcpp::Time(0);
last_set_pose_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you really want to set the clock? This would make it so that you always use the ROS clock and not the other options.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you launched a test with use_sim_time it would use ROS time

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you don't set it, it defaults to RCL_SYSTEM_TIME, right? And if I don't set this, the code throws an exception, because we end up comparing the current message time stamp (which is in ROS time) with this, which is in system time.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Wouldn't the tests automatically do that?

Also, is this not like ROS1? Does ROS time not == system time for a live system, and sim time in bags?

Copy link
Collaborator Author

@ayrton04 ayrton04 Feb 5, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just linking this so we're talking about the same stuff. You obviously have been working with ROS2 a lot more than I have, so apologies if I have this wrong.

https://design.ros2.org/articles/clock_and_time.html

The ROSTime will report the same as SystemTime when a ROS Time Source is not active. When the ROS time source is active ROSTime will return the latest value reported by the Time Source. ROSTime is considered active when the parameter use_sim_time is set on the node.

This is just like ROS1. The problem here is that if you don't provide constructor arguments, you'll get the second constructor signature here:

https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Time.html
https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Time.html#a683d5111f099d16532f10fcd82e8f5af

That means an uninitialized rclcpp::Time object will default to SYSTEM_TIME, as I read it. And that was the issue: the new last_published_stamp_ was not being initialized with a different constructor, so it was defaulting to RCL_SYSTEM_TIME, but we later compared it to a message stamp, which is obviously going to be of type RCL_ROS_TIME. That throws an exception, causing all my tests to fail.

In any case, tests are now all passing. 🥳

last_diag_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
latest_control_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
last_published_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME);

// clear tf buffer to avoid TF_OLD_DATA errors
tf_buffer_->clear();
Expand All @@ -143,9 +148,6 @@ void RosFilter<T>::reset()

// reset filter to uninitialized state
filter_.reset();

// clear all waiting callbacks
// ros::getGlobalCallbackQueue()->clear();
}

template<typename T>
Expand Down Expand Up @@ -2129,15 +2131,15 @@ void RosFilter<T>::periodicUpdate()
}
}

// Retain the last published stamp so we can detect repeated transforms in future cycles
last_published_stamp_ = filtered_position->header.stamp;

// Fire off the position and the transform
if (!corrected_data)
{
position_pub_->publish(std::move(filtered_position));
}

// Retain the last published stamp so we can detect repeated transforms in future cycles
last_published_stamp_ = filtered_position->header.stamp;

if (print_diagnostics_) {
freq_diag_->tick();
}
Expand Down