diff --git a/.gitignore b/.gitignore index e5d38e6e8..57577b288 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ doc/html *.cproject *.project *.settings +*_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt index 4357d76e2..e6649f781 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,10 +41,12 @@ find_package(GeographicLib REQUIRED) set(library_name rl_lib) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/State.msg" "srv/FromLL.srv" "srv/GetState.srv" "srv/SetDatum.srv" "srv/SetPose.srv" + "srv/SetState.srv" "srv/ToggleFilterProcessing.srv" "srv/ToLL.srv" DEPENDENCIES diff --git a/include/robot_localization/ros_filter.hpp b/include/robot_localization/ros_filter.hpp index e32ffeddc..583432ad6 100644 --- a/include/robot_localization/ros_filter.hpp +++ b/include/robot_localization/ros_filter.hpp @@ -34,6 +34,7 @@ #define ROBOT_LOCALIZATION__ROS_FILTER_HPP_ #include +#include #include #include @@ -279,6 +280,23 @@ class RosFilter : public rclcpp::Node //! void initialize(); + //! @brief Topic callback for manually setting/resetting the whole internal state estimate of the filter (not just the pose) + //! + //! @param[in] msg - Custom message with pose, twist, and accel information + void setStateCallback( + const robot_localization::msg::State::SharedPtr msg); + + //! @brief Service callback for manually setting/resetting the whole internal state estimate of the filter (not just the pose) + //! + //! @param[in] request - Custom service request with pose, twist, and accel information + //! @return true if successful, false if not + bool setStateSrvCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + + //! @brief Callback method for manually setting/resetting the internal pose //! estimate //! @param[in] msg - The ROS stamped pose with covariance message to take in @@ -728,6 +746,12 @@ class RosFilter : public rclcpp::Node //! rclcpp::Subscription::SharedPtr control_sub_; + //! @brief Subscribes to the set_state topic + //! Message type is robot_localization/State. + //! + rclcpp::Subscription::SharedPtr + set_state_sub_; + //! @brief Subscribes to the set_pose topic (usually published from rviz). //! Message type is geometry_msgs/PoseWithCovarianceStamped. //! @@ -740,6 +764,12 @@ class RosFilter : public rclcpp::Node rclcpp::Service::SharedPtr set_pose_service_; + //! @brief Service that allows another node to change the current state + //! (not just the pose) and recieve a confirmation. Uses a custom SetState service. + //! + rclcpp::Service::SharedPtr + set_state_service_; + //! @brief Service that allows another node to enable the filter. Uses a //! standard Empty service. //! diff --git a/msg/State.msg b/msg/State.msg new file mode 100644 index 000000000..7c64776eb --- /dev/null +++ b/msg/State.msg @@ -0,0 +1,4 @@ +geometry_msgs/PoseWithCovarianceStamped pose +geometry_msgs/TwistWithCovarianceStamped twist +geometry_msgs/AccelWithCovarianceStamped accel +bool[15] update_vector \ No newline at end of file diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 1315490cb..61335838e 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -51,7 +51,7 @@ #include #include #include - +#include namespace robot_localization { using namespace std::chrono_literals; @@ -110,6 +110,7 @@ RosFilter::~RosFilter() topic_subs_.clear(); timer_.reset(); set_pose_sub_.reset(); + set_state_sub_.reset(); control_sub_.reset(); tf_listener_.reset(); tf_buffer_.reset(); @@ -1035,6 +1036,12 @@ void RosFilter::loadParams() "set_pose", rclcpp::QoS(1), std::bind(&RosFilter::setPoseCallback, this, std::placeholders::_1)); + // Create a subscriber for manually setting/resetting state + set_state_sub_ = + this->create_subscription( + "set_state", rclcpp::QoS(1), + std::bind(&RosFilter::setStateCallback, this, std::placeholders::_1)); + // Create a service for manually setting/resetting pose set_pose_service_ = this->create_service( @@ -1042,6 +1049,13 @@ void RosFilter::loadParams() &RosFilter::setPoseSrvCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + // Create a service more manually setting/resetting the internal state + set_state_service_ = + this->create_service( + "set_state", std::bind( + &RosFilter::setStateSrvCallback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + // Create a service for manually enabling the filter enable_filter_srv_ = this->create_service( @@ -2243,6 +2257,148 @@ bool RosFilter::setPoseSrvCallback( return true; } +template +void RosFilter::setStateCallback(const robot_localization::msg::State::SharedPtr msg) +{ + std::string topic_name("set_state"); + std::vector update_vector(msg->update_vector.begin(), msg->update_vector.end()); + + // Get rid of any initial poses (pretend we've never had a measurement) + initial_measurements_.clear(); + previous_measurements_.clear(); + previous_measurement_covariances_.clear(); + + clearMeasurementQueue(); + + filter_state_history_.clear(); + measurement_history_.clear(); + + // Also set the last set pose time, so we ignore all messages + // that occur before it + last_set_pose_time_ = msg->pose.header.stamp; + + // Set the state vector to the reported pose + Eigen::VectorXd measurement(STATE_SIZE); + Eigen::MatrixXd measurement_covariance(STATE_SIZE, STATE_SIZE); + + // We only measure pose variables, so initialize the vector to 0 + measurement.setZero(); + + // Set this to the identity and let the message reset it + measurement_covariance.setIdentity(); + measurement_covariance *= 1e-6; + + Eigen::Matrix msg_pose_covariance; + msg_pose_covariance.setZero(); + std::copy_n(msg->pose.pose.covariance.begin(), msg->pose.pose.covariance.size(), msg_pose_covariance.data()); + if (msg_pose_covariance.isApprox(msg_pose_covariance.transpose())){ + Eigen::VectorXcd eigenvalues = msg_pose_covariance.eigenvalues(); + bool all_real_and_nonnegative = true; + for(unsigned int i =0; i < eigenvalues.size(); i++) { + std::complex< double > eigval = eigenvalues[i]; + all_real_and_nonnegative = all_real_and_nonnegative + && eigval.real()>0.0 && std::abs(eigval.imag())<1E-6; + } + if(all_real_and_nonnegative){ + RCLCPP_INFO(get_logger(), "Setting pose"); + // Prepare the pose data (really just using this to transform it into the + // target frame). Twist data is going to get zeroed out, but we'll set it later. + std::shared_ptr pose_ptr = std::make_shared(msg->pose); + preparePose( + pose_ptr, topic_name, world_frame_id_, false, false, false, + update_vector, measurement, measurement_covariance); + }else{ + RCLCPP_INFO(get_logger(), "Not setting pose because pose covariance is not positive definite"); + } + }else{ + RCLCPP_INFO(get_logger(), "Not setting pose because pose covariance is not symmetric"); + } + + Eigen::Matrix msg_twist_covariance; + msg_twist_covariance.setZero(); + std::copy_n(msg->twist.twist.covariance.begin(), msg->twist.twist.covariance.size(), msg_twist_covariance.data()); + if (msg_twist_covariance.isApprox(msg_twist_covariance.transpose())){ + Eigen::VectorXcd eigenvalues = msg_twist_covariance.eigenvalues(); + bool all_real_and_nonnegative = true; + for(unsigned int i =0; i < eigenvalues.size(); i++) { + std::complex< double > eigval = eigenvalues[i]; + all_real_and_nonnegative = all_real_and_nonnegative + && eigval.real()>0.0 && std::abs(eigval.imag())<1E-6; + } + if(all_real_and_nonnegative) { + // Prepare the twist data. + RCLCPP_INFO(get_logger(), "Setting twist"); + std::shared_ptr twist_ptr = std::make_shared(msg->twist); + prepareTwist( + twist_ptr, topic_name, base_link_frame_id_, update_vector, + measurement, measurement_covariance); + }else{ + RCLCPP_INFO(get_logger(), "Not setting twist because twist covariance is not positive definite"); + } + }else{ + RCLCPP_INFO(get_logger(), "Not setting twist because twist covariance is not symmetric"); + } + + Eigen::Matrix msg_accel_covariance_full; + msg_accel_covariance_full.setZero(); + std::copy_n(msg->accel.accel.covariance.begin(), msg->accel.accel.covariance.size(), msg_accel_covariance_full.data()); + Eigen::Matrix msg_accel_covariance = msg_accel_covariance_full.block<3,3>(0,0); + if (msg_accel_covariance.isApprox(msg_accel_covariance.transpose())) { + Eigen::Vector3cd eigenvalues = msg_accel_covariance.eigenvalues(); + bool all_real_and_nonnegative = true; + for(unsigned int i =0; i < eigenvalues.size(); i++) { + std::complex< double > eigval = eigenvalues[i]; + all_real_and_nonnegative = all_real_and_nonnegative + && eigval.real()>0.0 && std::abs(eigval.imag())<1E-6; + } + if(all_real_and_nonnegative){ + // Prepare the acceleration data. + RCLCPP_INFO(get_logger(), "Setting acceleration"); + std::shared_ptr imu_ptr(new sensor_msgs::msg::Imu); + std::vector update_vector_imu(msg->update_vector.begin(), msg->update_vector.end()); + update_vector_imu[StateMemberVroll]=update_vector_imu[StateMemberVpitch]=update_vector_imu[StateMemberVyaw]= + update_vector_imu[StateMemberRoll]=update_vector_imu[StateMemberPitch]=update_vector_imu[StateMemberYaw]=false; + imu_ptr->set__header(msg->accel.header); + imu_ptr->set__linear_acceleration(msg->accel.accel.accel.linear); + std::copy_n(msg_accel_covariance.data(), msg_accel_covariance.size(), imu_ptr->linear_acceleration_covariance.data()); + prepareAcceleration( + imu_ptr, topic_name, base_link_frame_id_, update_vector_imu, + measurement, measurement_covariance); + }else{ + RCLCPP_INFO(get_logger(), "Not setting acceleration because acceleration covariance is not positive definite"); + } + }else{ + RCLCPP_INFO(get_logger(), "Not setting acceleration because acceleration covariance is not symmetric"); + } + // For the state + filter_.setState(measurement); + filter_.setEstimateErrorCovariance(measurement_covariance); + + filter_.setLastMeasurementTime(this->now()); +} +template +bool RosFilter::setStateSrvCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_DEBUG(get_logger(), "--setStateSrvCallback--"); + RCLCPP_DEBUG(get_logger(), "sequence number: %lld. writer guid: %s", request_header->sequence_number, std::string((char*)&(request_header->writer_guid[0])).c_str()); + try + { + std::shared_ptr state_ptr = std::make_shared(request->state); + setStateCallback(state_ptr); + response->set__success(true); + } + catch(const std::exception& e) + { + RCLCPP_ERROR(get_logger(), "Unable to set EKF state: %s" , e.what()); + response->set__success(false); + } + return response->success; + +} + template bool RosFilter::enableFilterSrvCallback( const std::shared_ptr, diff --git a/srv/SetState.srv b/srv/SetState.srv new file mode 100644 index 000000000..c94dac351 --- /dev/null +++ b/srv/SetState.srv @@ -0,0 +1,3 @@ +State state +--- +bool success \ No newline at end of file