Removal of dead code and better debug output #756
Conversation
WIP Added parameter revert Rebase error Cleanup Changed to crash. Added warning after 0.5s
|
I don't necessarily think its terribly useful to open a PR called WIP without any context / information? |
My apologies, I'm new to the OS dev style. I wasn't intending on anyone to look at it yet. If you have recommendations on best practices let me know. It should be polished up shortly. |
|
Usually you wouldn't open the pull request until you were ready for folks to look at it 😄 (or are the core developer of the work, so the main repo is your sandbox) |
|
|
|
Sorry for the false start. Should be good now. |
|
Thanks for this. Will review soon. |
| @@ -242,6 +242,10 @@ If any of your sensors produce data with timestamps that are older than the most | |||
| ^^^^^^^^^^^^^^^ | |||
| If ``smooth_lagged_data`` is set to *true*, this parameter specifies the number of seconds for which the filter will retain its state and measurement history. This value should be at least as large as the time delta between your lagged measurements and the current time. | |||
|
|
|||
| ~max_future_queue_size | |||
| ^^^^^^^^^^^^^^^ | |||
| This parameter specifies the number of total data samples, arriving between filter runs, the filter will retain. This is to ensure excessive memory is not used as would happen if incoming data is badly time stamped or the filter ``frequency`` is badly set. | |||
There was a problem hiding this comment.
Maybe "arriving between execution cycles" instead of "arriving between filter runs"? Also, could we not control this by limiting the queue sizes on the actual sensor inputs, e.g., use the existing <input name>_queue_size parameters? With this option, if you have one extremely high frequency sensor and one very low frequency sensor, it seems possible that the wrong setting here will result in one sensor getting completely ignored.
| //! @param[in] stamp - The timestamp associated with this message. | ||
| //! @return true if the measrement is newer than all known measurements. | ||
| //! | ||
| bool validateMeasurementOrdering(const std::string& topicName, |
There was a problem hiding this comment.
Not sure how some of these other methods retained camelCase variable names, but we should try to stick to ROS 2 standards and use snake_case, e.g., topic_name.
| //! @param[in] stamp - The timestamp associated with this message. | ||
| //! @return true if the measrement is newer than all known measurements. | ||
| //! | ||
| bool validateMeasurementTime(const std::string& topicName, |
| // that arrive with an older timestamp | ||
| if (last_set_pose_time_ >= msg->header.stamp) { | ||
| const sensor_msgs::msg::Imu::SharedPtr msg, | ||
| const CallbackData& callback_data, const std::string& target_frame) { |
There was a problem hiding this comment.
Any reason to change the style here? If it passed linting before, I'd prefer that we leave it alone and minimize changes. I'm pretty sure ROS 2 standards specify that reference types be given as TypeName & variable_name, which is what was there before. And target_frame was on its own line.
| "Commanded velocities must be given in the robot's body frame (" << | ||
| base_link_frame_id_ << "). Message frame was " << | ||
| msg->header.frame_id << "\n"; | ||
| std::cerr |
There was a problem hiding this comment.
Can we use the ROS 2 logging macros here? I think I updated these in the rolling branch.
|
|
||
| if ((time - measurement_queue_.top()->time_).nanoseconds() / 1e9 > | ||
| kMaxQueueTimeS) { | ||
| RCLCPP_ERROR(this->get_logger(), "Warning: messages are queued at least " |
There was a problem hiding this comment.
We're using RCLCPP_ERROR and then printing out the word Warning:. If it's a warning, I'd remove the Warning: and use RCLCPP_WARN. If it's an error, then I'd just remove the Warning:.
| @@ -83,6 +83,7 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) | |||
| yaw_offset_(0.0), | |||
| zero_altitude_(false) | |||
| { | |||
|
|
|||
| // transform from " | ||
| // << odom_frame_id_ << "->" << | ||
| // base_link_frame_id_); | ||
| RF_DEBUG("Filter could not get odom to baselink ids. \n") |
There was a problem hiding this comment.
| RF_DEBUG("Filter could not get odom to baselink ids. \n") | |
| RF_DEBUG("Could not obtain transform from " << odom_frame_id_ << | |
| " to " << base_link_frame_id_ << "\n") |
| if (getFilteredOdometryMessage(filtered_position.get())) { | ||
| RF_DEBUG("Inside main loop function \n") |
There was a problem hiding this comment.
This is a little ambiguous. Maybe we can think of something a little clearer?
| filter_utilities::toSec(last_history_state->latest_control_time_) << | ||
| ".\n"); | ||
|
|
||
| // ROS_WARN_STREAM_DELAYED_THROTTLE(history_length_, "Could not revert " |
There was a problem hiding this comment.
I think I put a bunch of these back and just used the RCLCPP equivalents in the rolling branch.
This PR does a few things:
This was primarily borne out from difficulties I had with the package where the filter was failing silently