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

ROS1 recent feature port #613

Merged
merged 11 commits into from
Mar 22, 2021
Merged

ROS1 recent feature port #613

merged 11 commits into from
Mar 22, 2021

Conversation

ayrton04
Copy link
Collaborator

@ayrton04 ayrton04 commented Dec 4, 2020

I have one more to add to this.

@SteveMacenski
Copy link
Collaborator

Ok, let me know when you want me to take a look

@ayrton04
Copy link
Collaborator Author

ayrton04 commented Dec 11, 2020

OK, so here's a list of the commits and their equivalent ROS1 commits:

de19931 : 0dc08ed
07ee6c2 : f9a20dc
c954475 : 9277d98
b4017d2 : f760c01
b079b73 : 1912687
08d7fca : 1bfc112
fe2a8b0 : 660793f and 0009b57

@ayrton04 ayrton04 marked this pull request as ready for review December 11, 2020 11:40
@ayrton04
Copy link
Collaborator Author

I still need to get my ROS2 workspace properly dusted off again and build this to make sure it's OK. And then I need to find a way to run the stuff with the local Cartesian coordinate frame.

@ayrton04
Copy link
Collaborator Author

Does the build farm not automatically test commits for ROS2 PRs?

@SteveMacenski
Copy link
Collaborator

It should if you setup the webhooks. You set it up for ROS1 with http://build.ros.org/ghprbhook/ addresses, you need to also enable it for ROS2 with build.ros2.org.... I don't have the settings tab on RL to change myself, but this is setup for ROS1/ROS2 building on another package I manage.

image

@ayrton04
Copy link
Collaborator Author

ayrton04 commented Jan 6, 2021

Yeah, the build farm should automatically add those, because I gave it permission to. And it did, but for some reason, they're showing up as reporting 503 errors:

image

@SteveMacenski
Copy link
Collaborator

I do not think the build farm automatically adds those, I always have to add those build.ros.org webhooks into my projects and are listed on some instructions somewhere. Anyhow, I'm also getting that warning symbol on other projects, I think something is just down right now.

Did you add that webhook just now or has it been there for a long time? If a long time, then I'm not sure why it wasn't triggering PR builds..

@SteveMacenski
Copy link
Collaborator

I think if you can build / run tests on foxy we can call it good since this is having issues. Let me know if you need me to do it instead.

@ayrton04
Copy link
Collaborator Author

ayrton04 commented Jan 8, 2021

Apparently the webhook URLs needed to be updated to use https instead of http. I've done so. Will close and reopen this to see if it triggers anything.

@ayrton04 ayrton04 closed this Jan 8, 2021
@ayrton04 ayrton04 reopened this Jan 8, 2021
@ayrton04
Copy link
Collaborator Author

ayrton04 commented Jan 8, 2021

Heh, looks like I have some things to fix. Will get my environment back up.

@SteveMacenski
Copy link
Collaborator

whoop, linting 😅

@ayrton04
Copy link
Collaborator Author

ayrton04 commented Feb 4, 2021

A bit more than linting, it seems. The last published message time wasn't be initialized to use ROS time, and so was defaulting to system time. Comparing the two was a problem. Moreover, I was publishing the filtered position with a std::move call, so filteredPosition became invalid after that.

Neither of these issues are present in the ROS1 branches, so no worries there.

We still get the occasional failure with some of the I/O tests. I really need to run those down.

@@ -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

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. 🥳

@SteveMacenski
Copy link
Collaborator

05:14:08 39: /tmp/ws/src/robot_localization/src/ros_filter.cpp:2061:  Should have a space between // and comment  [whitespace/comments] [4]


05:14:11 41: --- src/navsat_transform.cpp
05:14:11 41: +++ src/navsat_transform.cpp.uncrustify
05:14:11 41: @@ -110,5 +110,7 @@
05:14:11 41: -  broadcast_cartesian_transform_ = this->declare_parameter("broadcast_utm_transform", broadcast_cartesian_transform_);
05:14:11 41: -
05:14:11 41: -  if (broadcast_cartesian_transform_)
05:14:11 41: -  {
05:14:11 41: -    RCLCPP_WARN(this->get_logger(), "Parameter 'broadcast_utm_transform' has been deprecated. Please use "
05:14:11 41: +  broadcast_cartesian_transform_ = this->declare_parameter(
05:14:11 41: +    "broadcast_utm_transform",
05:14:11 41: +    broadcast_cartesian_transform_);
05:14:11 41: +
05:14:11 41: +  if (broadcast_cartesian_transform_) {
05:14:11 41: +    RCLCPP_WARN(
05:14:11 41: +      this->get_logger(), "Parameter 'broadcast_utm_transform' has been deprecated. Please use "
05:14:11 41: @@ -116,3 +118 @@
05:14:11 41: -  }
05:14:11 41: -  else
05:14:11 41: -  {
05:14:11 41: +  } else {
05:14:11 41: @@ -124 +124 @@
05:14:11 41: -  this->declare_parameter(
05:14:11 41: +    this->declare_parameter(
05:14:11 41: @@ -128,3 +128,3 @@
05:14:11 41: -  if (broadcast_cartesian_transform_as_parent_frame_)
05:14:11 41: -  {
05:14:11 41: -    RCLCPP_WARN(this->get_logger(), "Parameter 'broadcast_utm_transform_as_parent_frame' has been deprecated. Please "
05:14:11 41: +  if (broadcast_cartesian_transform_as_parent_frame_) {
05:14:11 41: +    RCLCPP_WARN(
05:14:11 41: +      this->get_logger(), "Parameter 'broadcast_utm_transform_as_parent_frame' has been deprecated. Please "
05:14:11 41: @@ -132,3 +132 @@
05:14:11 41: -  }
05:14:11 41: -  else
05:14:11 41: -  {
05:14:11 41: +  } else {
05:14:11 41: @@ -137,2 +135,2 @@
05:14:11 41: -        "broadcast_cartesian_transform_as_parent_frame",
05:14:11 41: -        broadcast_cartesian_transform_as_parent_frame_);
05:14:11 41: +      "broadcast_cartesian_transform_as_parent_frame",
05:14:11 41: +      broadcast_cartesian_transform_as_parent_frame_);
05:14:11 41: @@ -314 +312,3 @@
05:14:11 41: -    cartesian_world_transform_.mult(transform_world_pose_yaw_only, cartesian_pose_with_orientation.inverse());
05:14:11 41: +    cartesian_world_transform_.mult(
05:14:11 41: +      transform_world_pose_yaw_only,
05:14:11 41: +      cartesian_pose_with_orientation.inverse());
05:14:11 41: @@ -421,2 +421 @@
05:14:11 41: -  if (use_local_cartesian_)
05:14:11 41: -  {
05:14:11 41: +  if (use_local_cartesian_) {
05:14:11 41: @@ -430,3 +429 @@
05:14:11 41: -  }
05:14:11 41: -  else
05:14:11 41: -  {
05:14:11 41: +  } else {
05:14:11 41: @@ -829,2 +826 @@
05:14:11 41: -  if (use_local_cartesian_)
05:14:11 41: -  {
05:14:11 41: +  if (use_local_cartesian_) {
05:14:11 41: @@ -833 +829,3 @@
05:14:11 41: -    gps_local_cartesian_.Forward(msg->latitude, msg->longitude, msg->altitude, cartesian_x, cartesian_y, cartesian_z);
05:14:11 41: +    gps_local_cartesian_.Forward(
05:14:11 41: +      msg->latitude, msg->longitude, msg->altitude, cartesian_x,
05:14:11 41: +      cartesian_y, cartesian_z);
05:14:11 41: @@ -837,3 +835 @@
05:14:11 41: -  }
05:14:11 41: -  else
05:14:11 41: -  {
05:14:11 41: +  } else {
05:14:11 41: @@ -855 +851,2 @@
05:14:11 41: -    ((use_local_cartesian_)? "Local Cartesian" : "UTM"), utm_zone_.c_str(), cartesian_x, cartesian_y);
05:14:11 41: +    ((use_local_cartesian_) ? "Local Cartesian" : "UTM"),
05:14:11 41: +    utm_zone_.c_str(), cartesian_x, cartesian_y);
05:14:11 41: 


05:14:11 41: --- src/ros_filter.cpp
05:14:11 41: +++ src/ros_filter.cpp.uncrustify
05:14:11 41: @@ -1007,24 +1007,24 @@
05:14:11 41: -    "tf_prefix is " << tf_prefix <<
05:14:11 41: -    "\nmap_frame is " << map_frame_id_ <<
05:14:11 41: -    "\nodom_frame is " << odom_frame_id_ <<
05:14:11 41: -    "\nbase_link_frame is " << base_link_frame_id_ <<
05:14:11 41: -    "\nbase_link_output_frame is " << base_link_output_frame_id_ <<
05:14:11 41: -    "\nworld_frame is " << world_frame_id_ <<
05:14:11 41: -    "\ntransform_time_offset is " << filter_utilities::toSec(tf_time_offset_) <<
05:14:11 41: -    "\ntransform_timeout is " << filter_utilities::toSec(tf_timeout_) <<
05:14:11 41: -    "\nfrequency is " << frequency_ <<
05:14:11 41: -    "\nsensor_timeout is " << filter_utilities::toSec(filter_.getSensorTimeout()) <<
05:14:11 41: -    "\ntwo_d_mode is " << (two_d_mode_ ? "true" : "false") <<
05:14:11 41: -    "\nsmooth_lagged_data is " << (smooth_lagged_data_ ? "true" : "false") <<
05:14:11 41: -    "\nhistory_length is " << filter_utilities::toSec(history_length_) <<
05:14:11 41: -    "\nuse_control is " << use_control_ <<
05:14:11 41: -    "\ncontrol_config is " << control_update_vector <<
05:14:11 41: -    "\ncontrol_timeout is " << control_timeout <<
05:14:11 41: -    "\nacceleration_limits are " << acceleration_limits <<
05:14:11 41: -    "\nacceleration_gains are " << acceleration_gains <<
05:14:11 41: -    "\ndeceleration_limits are " << deceleration_limits <<
05:14:11 41: -    "\ndeceleration_gains are " << deceleration_gains <<
05:14:11 41: -    "\ninitial state is " << filter_.getState() <<
05:14:11 41: -    "\ndynamic_process_noise_covariance is " << dynamic_process_noise_covariance <<
05:14:11 41: -    "\npermit_corrected_publication is " << permit_corrected_publication_ <<
05:14:11 41: -    "\nprint_diagnostics is " << print_diagnostics_ << "\n");
05:14:11 41: +      "tf_prefix is " << tf_prefix <<
05:14:11 41: +      "\nmap_frame is " << map_frame_id_ <<
05:14:11 41: +      "\nodom_frame is " << odom_frame_id_ <<
05:14:11 41: +      "\nbase_link_frame is " << base_link_frame_id_ <<
05:14:11 41: +      "\nbase_link_output_frame is " << base_link_output_frame_id_ <<
05:14:11 41: +      "\nworld_frame is " << world_frame_id_ <<
05:14:11 41: +      "\ntransform_time_offset is " << filter_utilities::toSec(tf_time_offset_) <<
05:14:11 41: +      "\ntransform_timeout is " << filter_utilities::toSec(tf_timeout_) <<
05:14:11 41: +      "\nfrequency is " << frequency_ <<
05:14:11 41: +      "\nsensor_timeout is " << filter_utilities::toSec(filter_.getSensorTimeout()) <<
05:14:11 41: +      "\ntwo_d_mode is " << (two_d_mode_ ? "true" : "false") <<
05:14:11 41: +      "\nsmooth_lagged_data is " << (smooth_lagged_data_ ? "true" : "false") <<
05:14:11 41: +      "\nhistory_length is " << filter_utilities::toSec(history_length_) <<
05:14:11 41: +      "\nuse_control is " << use_control_ <<
05:14:11 41: +      "\ncontrol_config is " << control_update_vector <<
05:14:11 41: +      "\ncontrol_timeout is " << control_timeout <<
05:14:11 41: +      "\nacceleration_limits are " << acceleration_limits <<
05:14:11 41: +      "\nacceleration_gains are " << acceleration_gains <<
05:14:11 41: +      "\ndeceleration_limits are " << deceleration_limits <<
05:14:11 41: +      "\ndeceleration_gains are " << deceleration_gains <<
05:14:11 41: +      "\ninitial state is " << filter_.getState() <<
05:14:11 41: +      "\ndynamic_process_noise_covariance is " << dynamic_process_noise_covariance <<
05:14:11 41: +      "\npermit_corrected_publication is " << permit_corrected_publication_ <<
05:14:11 41: +      "\nprint_diagnostics is " << print_diagnostics_ << "\n");
05:14:11 41: @@ -2138,2 +2138 @@
05:14:11 41: -    if (!corrected_data)
05:14:11 41: -    {
05:14:11 41: +    if (!corrected_data) {
05:14:11 41: @@ -2151 +2150 @@
05:14:11 41: -      getFilteredAccelMessage(filtered_acceleration.get()))
05:14:11 41: +    getFilteredAccelMessage(filtered_acceleration.get()))
05:14:11 41: 

Looks like the remaining test failures are just linting

@ayrton04
Copy link
Collaborator Author

ayrton04 commented Feb 4, 2021

Yeah, I fixed them locally. About to push.

@ayrton04 ayrton04 merged commit 3dbd48a into foxy-devel Mar 22, 2021
@ayrton04 ayrton04 deleted the ros1-feature-port branch March 22, 2021 09:57
Timple pushed a commit to nobleo/robot_localization that referenced this pull request Apr 13, 2021
* navsat_transform diagram to address cra-ros-pkg#550 (cra-ros-pkg#570)

* add diagram for navsat_transform

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>

* add diagram to tutorial

Signed-off-by: Mabel Zhang <mabel@openrobotics.org>

* Fix frame id of imu in differential mode, closes cra-ros-pkg#482

* Added local Cartesian option to navsat_transform

* Fix navsat_transform issue with starting on uneven terrain

* Fix typo in navsat_transform_node.rst (cra-ros-pkg#588)

"prodives" -> "provides"

* Fix sign error in dFY_dP in transfer function jacobian

* Making repeated state publication optional (cra-ros-pkg#595)

* PR feedback

* Fixing build issues

* Fixing stamp issues

* Linting and uncrustifying

Co-authored-by: Mabel Zhang <mabel.m.zhang@gmail.com>
Co-authored-by: Jeffrey Kane Johnson <jeff@mapless.ai>
"Please use 'broadcast_cartesian_transform' instead.");
} else {
broadcast_cartesian_transform_ =
this->declare_parameter("broadcast_utm_transform", broadcast_cartesian_transform_);
Copy link
Contributor

Choose a reason for hiding this comment

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

Shouldn't this be broadcast_cartesian_transform then?

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

Yep, thanks.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

5 participants