diff --git a/rm_common/CHANGELOG.rst b/rm_common/CHANGELOG.rst index c86fb9fb..8b301245 100644 --- a/rm_common/CHANGELOG.rst +++ b/rm_common/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rm_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.9 (2022-3-28) +------------------ +* Add imu_filter and deprecated imu_extra_handle(Since the update frequency of the control loop is not stable, some of + the camera trigger signals of imu will be lost. We put the imu filter down to the hardware resource layer, so + imu_extra_handle is breaking. ) +* Add tof sensor interface +* Contributors: Edwinlinks, Jie j, QiayuanLiao, yezi + 0.1.8 (2021-12-7) ------------------ * Merge branch 'master' into master diff --git a/rm_common/CMakeLists.txt b/rm_common/CMakeLists.txt index c87c9556..13b967fc 100644 --- a/rm_common/CMakeLists.txt +++ b/rm_common/CMakeLists.txt @@ -20,6 +20,8 @@ find_package(catkin REQUIRED geometry_msgs control_msgs controller_manager_msgs + imu_complementary_filter + imu_filter_madgwick realtime_tools dynamic_reconfigure ) @@ -34,6 +36,8 @@ catkin_package( geometry_msgs control_msgs controller_manager_msgs + imu_complementary_filter + imu_filter_madgwick roscpp dynamic_reconfigure DEPENDS @@ -47,7 +51,7 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -file(GLOB_RECURSE sources "src/*.cpp" "src/decision/*.cpp") +file(GLOB_RECURSE sources "src/*.cpp" "src/decision/*.cpp" "src/filter/*.cpp") add_library(rm_common SHARED ${sources} include/rm_common/referee/data.h) #add_executable(test_traj test/test_traj.cpp) diff --git a/rm_common/include/rm_common/filters/imu_complementary_filter.h b/rm_common/include/rm_common/filters/imu_complementary_filter.h new file mode 100644 index 00000000..a6d12d97 --- /dev/null +++ b/rm_common/include/rm_common/filters/imu_complementary_filter.h @@ -0,0 +1,26 @@ +// +// Created by yezi on 2022/3/26. +// + +#pragma once + +#include +#include + +namespace rm_common +{ +class ImuComplementaryFilter : public ImuFilterBase +{ +public: + ImuComplementaryFilter() = default; + void getOrientation(double& q0, double& q1, double& q2, double& q3) override; + +private: + void filterUpdate(double ax, double ay, double az, double wx, double wy, double wz, double dt) override; + bool getFilterParam(XmlRpc::XmlRpcValue& imu_data) override; + // Parameters: + bool use_mag_; + // State variables: + imu_tools::ComplementaryFilter filter_; +}; +} // namespace rm_common diff --git a/rm_common/include/rm_common/filters/imu_filter_base.h b/rm_common/include/rm_common/filters/imu_filter_base.h new file mode 100644 index 00000000..45b1afa4 --- /dev/null +++ b/rm_common/include/rm_common/filters/imu_filter_base.h @@ -0,0 +1,33 @@ +// +// Created by yezi on 2022/3/26. +// + +#pragma once + +#include +#include +#include +#include +#include + +namespace rm_common +{ +class ImuFilterBase +{ +public: + bool init(XmlRpc::XmlRpcValue& imu_data, const std::string& name); + void update(ros::Time time, double* accel, double* omega, double* ori, double* accel_cov, double* omega_cov, + double* ori_cov, double temp, bool camera_trigger); + virtual void getOrientation(double& q0, double& q1, double& q2, double& q3) = 0; + +protected: + virtual bool getFilterParam(XmlRpc::XmlRpcValue& imu_data) = 0; + virtual void filterUpdate(double ax, double ay, double az, double wx, double wy, double wz, double dt) = 0; + ros::Time last_update_; + bool initialized_filter_{ false }; + std::string frame_id_; + std::shared_ptr > imu_data_pub_; + std::shared_ptr > imu_temp_pub_; + std::shared_ptr > trigger_time_pub_; +}; +} // namespace rm_common diff --git a/rm_common/include/rm_common/hardware_interface/imu_extra_interface.h b/rm_common/include/rm_common/hardware_interface/imu_extra_interface.h deleted file mode 100644 index 960a82a5..00000000 --- a/rm_common/include/rm_common/hardware_interface/imu_extra_interface.h +++ /dev/null @@ -1,118 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2021, Qiayuan Liao - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -// -// Created by qiayuan on 15/9/21. -// - -#pragma once - -#include -#include - -namespace rm_control -{ -class ImuExtraHandle -{ -public: - ImuExtraHandle() = default; - ImuExtraHandle(std::string name, double* orientation, bool* accel_update, bool* gyro_update, bool* camera_trigger, - const double* temperature) - : name_(std::move(name)) - , orientation_(orientation) - , accel_updated_(accel_update) - , gyro_updated_(gyro_update) - , camera_trigger_(camera_trigger) - , temperature_(temperature) - { - if (!accel_update) - throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + - "'. accel_update pointer is null."); - if (!gyro_update) - throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + - "'. gyro_update pointer is null."); - if (!camera_trigger) - throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + - "'. camera_trigger pointer is null."); - if (!temperature) - throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + - "'. camera_trigger pointer is null."); - } - std::string getName() const - { - return name_; - } - void setOrientation(double ori_x, double ori_y, double ori_z, double ori_w) - { - orientation_[0] = ori_x; - orientation_[1] = ori_y; - orientation_[2] = ori_z; - orientation_[3] = ori_w; - } - bool getAccelUpdated() const - { - assert(accel_updated_); - return *accel_updated_; - } - bool getGyroUpdated() const - { - assert(gyro_updated_); - return *gyro_updated_; - } - bool getCameraTrigger() const - { - assert(camera_trigger_); - return *camera_trigger_; - } - double getTemperature() const - { - assert(temperature_); - return *temperature_; - } - -private: - std::string name_; - double* orientation_; - bool* accel_updated_; - bool* gyro_updated_; - bool* camera_trigger_; - const double* temperature_{}; - // TODO: Add magnetic (double* ) -}; - -class ImuExtraInterface - : public hardware_interface::HardwareResourceManager -{ -}; - -} // namespace rm_control diff --git a/rm_common/package.xml b/rm_common/package.xml index 211466e1..35bc622a 100644 --- a/rm_common/package.xml +++ b/rm_common/package.xml @@ -1,7 +1,7 @@ rm_common - 0.1.8 + 0.1.9 The rm_common package qiayuan @@ -16,6 +16,8 @@ geometry_msgs realtime_tools rm_msgs + imu_complementary_filter + imu_filter_madgwick dynamic_reconfigure eigen control_msgs diff --git a/rm_common/src/filters.cpp b/rm_common/src/filter/filters.cpp similarity index 100% rename from rm_common/src/filters.cpp rename to rm_common/src/filter/filters.cpp diff --git a/rm_common/src/filter/imu_complementary_filter.cpp b/rm_common/src/filter/imu_complementary_filter.cpp new file mode 100644 index 00000000..112dac6b --- /dev/null +++ b/rm_common/src/filter/imu_complementary_filter.cpp @@ -0,0 +1,47 @@ +// +// Created by yezi on 2022/3/26. +// + +#include + +namespace rm_common +{ +void ImuComplementaryFilter::filterUpdate(double ax, double ay, double az, double wx, double wy, double wz, double dt) +{ + filter_.update(ax, ay, az, wx, wy, wz, dt); +} +void ImuComplementaryFilter::getOrientation(double& q0, double& q1, double& q2, double& q3) +{ + filter_.getOrientation(q0, q1, q2, q3); +} +bool ImuComplementaryFilter::getFilterParam(XmlRpc::XmlRpcValue& imu_data) +{ + double gain_acc; + double gain_mag; + bool do_bias_estimation; + double bias_alpha; + bool do_adaptive_gain; + auto it = imu_data.begin(); + use_mag_ = imu_data.hasMember("use_mag") && (bool)imu_data[it->first]["use_mag"]; + gain_acc = imu_data.hasMember("gain_acc") ? (double)imu_data[it->first]["gain_acc"] : 0.01; + gain_mag = imu_data.hasMember("gain_mag") ? (double)imu_data[it->first]["gain_mag"] : 0.01; + do_bias_estimation = !imu_data.hasMember("do_bias_estimation") || (bool)imu_data[it->first]["do_bias_estimation"]; + bias_alpha = imu_data.hasMember("bias_alpha") ? (double)imu_data[it->first]["bias_alpha"] : 0.01; + do_adaptive_gain = !imu_data.hasMember("do_adaptive_gain") || (bool)imu_data[it->first]["do_adaptive_gain"]; + filter_.setDoBiasEstimation(do_bias_estimation); + filter_.setDoAdaptiveGain(do_adaptive_gain); + if (!filter_.setGainAcc(gain_acc)) + ROS_WARN("Invalid gain_acc passed to ComplementaryFilter."); + if (use_mag_) + { + if (!filter_.setGainMag(gain_mag)) + ROS_WARN("Invalid gain_mag passed to ComplementaryFilter."); + } + if (do_bias_estimation) + { + if (!filter_.setBiasAlpha(bias_alpha)) + ROS_WARN("Invalid bias_alpha passed to ComplementaryFilter."); + } + return true; +} +} // namespace rm_common diff --git a/rm_common/src/filter/imu_filter_base.cpp b/rm_common/src/filter/imu_filter_base.cpp new file mode 100644 index 00000000..eb10f12c --- /dev/null +++ b/rm_common/src/filter/imu_filter_base.cpp @@ -0,0 +1,73 @@ +// +// Created by yezi on 2022/3/26. +// + +#include + +namespace rm_common +{ +bool ImuFilterBase::init(XmlRpc::XmlRpcValue& imu_data, const std::string& name) +{ + ros::NodeHandle nh("~"); + frame_id_ = (std::string)imu_data["frame_id"]; + getFilterParam(imu_data); + imu_data_pub_.reset(new realtime_tools::RealtimePublisher(nh, name + "/data", 100)); + imu_temp_pub_.reset(new realtime_tools::RealtimePublisher(nh, name + "/temperature", 100)); + trigger_time_pub_.reset( + new realtime_tools::RealtimePublisher(nh, name + "/trigger_time", 100)); + return true; +} + +void ImuFilterBase::update(ros::Time time, double* accel, double* omega, double* ori, double* accel_cov, + double* omega_cov, double* ori_cov, double temp, bool camera_trigger) +{ + if (!initialized_filter_) + { + initialized_filter_ = true; + last_update_ = time; + imu_data_pub_->msg_.header.frame_id = frame_id_; + return; + } + + // Update the filter. + filterUpdate(accel[0], accel[1], accel[2], omega[0], omega[1], omega[2], (time - last_update_).toSec()); + last_update_ = time; + getOrientation(ori[3], ori[0], ori[1], ori[2]); + if (camera_trigger) + { + if (imu_data_pub_->trylock()) + { + imu_data_pub_->msg_.header.stamp = time; + imu_data_pub_->msg_.angular_velocity.x = omega[0]; + imu_data_pub_->msg_.angular_velocity.y = omega[1]; + imu_data_pub_->msg_.angular_velocity.z = omega[2]; + imu_data_pub_->msg_.linear_acceleration.x = accel[0]; + imu_data_pub_->msg_.linear_acceleration.y = accel[1]; + imu_data_pub_->msg_.linear_acceleration.z = accel[2]; + imu_data_pub_->msg_.orientation.x = ori[0]; + imu_data_pub_->msg_.orientation.y = ori[1]; + imu_data_pub_->msg_.orientation.z = ori[2]; + imu_data_pub_->msg_.orientation.w = ori[3]; + imu_data_pub_->msg_.orientation_covariance = { ori_cov[0], 0., 0., 0., ori_cov[4], 0., 0., 0., ori_cov[8] }; + imu_data_pub_->msg_.angular_velocity_covariance = { omega_cov[0], 0., 0., 0., omega_cov[4], + 0., 0., 0., omega_cov[8] }; + imu_data_pub_->msg_.linear_acceleration_covariance = { accel_cov[0], 0., 0., 0., accel_cov[4], + 0., 0., 0., accel_cov[8] }; + imu_data_pub_->unlockAndPublish(); + } + if (trigger_time_pub_->trylock()) + { + trigger_time_pub_->msg_.header.stamp = time; + trigger_time_pub_->msg_.time_ref = time; + trigger_time_pub_->unlockAndPublish(); + } + if (imu_temp_pub_->trylock()) + { + imu_temp_pub_->msg_.header.stamp = time; + imu_temp_pub_->msg_.temperature = temp; + imu_temp_pub_->unlockAndPublish(); + } + } +} + +} // namespace rm_common diff --git a/rm_common/src/lp_filter.cpp b/rm_common/src/filter/lp_filter.cpp similarity index 100% rename from rm_common/src/lp_filter.cpp rename to rm_common/src/filter/lp_filter.cpp diff --git a/rm_control/CHANGELOG.rst b/rm_control/CHANGELOG.rst index 58e5f9b0..9e638752 100644 --- a/rm_control/CHANGELOG.rst +++ b/rm_control/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rm_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.9 (2022-3-28) +------------------ +* Separate rm_description out of rm_control +* Revert "Update package.xml" + This reverts commit a46a8b40dffc0c5fa61981d9ca862a7f1da7e559. +* Update package.xml +* Merge remote-tracking branch 'origin/master' +* Contributors: Jie j, YuuinIH + 0.1.8 (2021-12-7) ------------------ * Merge branch 'master' into gimbal/opti_or_simplify diff --git a/rm_control/package.xml b/rm_control/package.xml index ba360d6b..e30be63f 100644 --- a/rm_control/package.xml +++ b/rm_control/package.xml @@ -1,7 +1,7 @@ rm_control - 0.1.8 + 0.1.9 Meta package that contains package of rm_control. Qiayuan Liao diff --git a/rm_dbus/CHANGELOG.rst b/rm_dbus/CHANGELOG.rst index 0cd35736..3d376f1f 100644 --- a/rm_dbus/CHANGELOG.rst +++ b/rm_dbus/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rm_dbus ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.9 (2022-3-28) +------------------ +* Merge remote-tracking branch 'origin/master' +* Contributors: Jie j + 0.1.8 (2021-12-7) ------------------ * Merge branch 'master' into master diff --git a/rm_dbus/package.xml b/rm_dbus/package.xml index d1dd00e1..75f89659 100644 --- a/rm_dbus/package.xml +++ b/rm_dbus/package.xml @@ -1,7 +1,7 @@ rm_dbus - 0.1.8 + 0.1.9 A package that uses dbus to read remote control information Qiayuan Liao diff --git a/rm_gazebo/CHANGELOG.rst b/rm_gazebo/CHANGELOG.rst index 0228537d..ca17cd06 100644 --- a/rm_gazebo/CHANGELOG.rst +++ b/rm_gazebo/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rm_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.9 (2022-3-28) +------------------ +* Revert "Update package.xml" + This reverts commit 835659c320b5e1219ea9c61d04b4017a0c9a850a. +* Update package.xml +* Merge remote-tracking branch 'origin/master' +* Merge branch 'master' of github.com:ye-luo-xi-tui/rm_control +* Merge pull request `#24 `_ from ye-luo-xi-tui/gazebo + Load imu params into gazebo +* Load imu params into gazebo. +* Contributors: Jie j, QiayuanLiao, YuuinIH, yezi + 0.1.8 (2021-12-7) ------------------ * Fix imu inertia and add imu to balance diff --git a/rm_gazebo/package.xml b/rm_gazebo/package.xml index 5a28e306..0af3155f 100644 --- a/rm_gazebo/package.xml +++ b/rm_gazebo/package.xml @@ -1,7 +1,7 @@ rm_gazebo - 0.1.8 + 0.1.9 A template for ROS packages. Qiayuan Liao BSD diff --git a/rm_hw/CHANGELOG.rst b/rm_hw/CHANGELOG.rst index 9e0e2929..68837c26 100644 --- a/rm_hw/CHANGELOG.rst +++ b/rm_hw/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package rm_hw ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.9 (2022-3-28) +------------------ +* Deprecated imu_extra_handle and add imu_filter into hardware resource layer.(Since the update frequency of the control + loop is not stable, some of the camera trigger signals of imu will be lost. We put the imu filter down to the hardware + resource layer, so imu_extra_handle is breaking. ) +* Merge pull request `#32 `_ from Edwinlinks/tof_sensor_interface + Delete contents in brackets +* Delete contents in brackets +* Merge pull request `#29 `_ from Edwinlinks/tof_sensor_interface + Completed tof_sensor_interface +* Modified the reference order of header files and packet parsing of tof sensor, data type of dis_status +* Add tof sensor interface in rm_common, add parsing can frame in can_bus.cpp, and add TofSensor.msg in rm_msgs. +* Merge remote-tracking branch 'origin/master' +* Merge pull request `#26 `_ from ye-luo-xi-tui/master + Fix a bug in parse imu +* Fix a stupid bug. +* Contributors: Edwinlinks, Jie j, QiayuanLiao, yezi + 0.1.8 (2021-12-7) ------------------ * Fix End of files. diff --git a/rm_hw/config/hw_config_template.yaml b/rm_hw/config/hw_config_template.yaml index 3b058e73..246334e2 100644 --- a/rm_hw/config/hw_config_template.yaml +++ b/rm_hw/config/hw_config_template.yaml @@ -20,3 +20,11 @@ rm_hw: orientation_covariance_diagonal: [ 0.0012, 0.0012, 0.0012 ] angular_velocity_covariance: [ 0.0004, 0.0004, 0.0004 ] linear_acceleration_covariance: [ 0.01, 0.01, 0.01 ] + angular_vel_coeff: 0.0010652644 + accel_coeff: 0.0017944335 + temp_coeff: 0.125 + temp_offset: 23.0 + filter: complementary + do_bias_estimation: false + do_adaptive_gain: true + gain_acc: 0.0003 diff --git a/rm_hw/include/rm_hw/hardware_interface/hardware_interface.h b/rm_hw/include/rm_hw/hardware_interface/hardware_interface.h index 91b5bc8f..43d48e74 100644 --- a/rm_hw/include/rm_hw/hardware_interface/hardware_interface.h +++ b/rm_hw/include/rm_hw/hardware_interface/hardware_interface.h @@ -57,7 +57,6 @@ #include #include -#include #include #include @@ -169,7 +168,6 @@ class RmRobotHW : public hardware_interface::RobotHW hardware_interface::EffortActuatorInterface effort_act_interface_; rm_control::RobotStateInterface robot_state_interface_; hardware_interface::ImuSensorInterface imu_sensor_interface_; - rm_control::ImuExtraInterface imu_extra_interface_; std::unique_ptr transmission_loader_{}; transmission_interface::RobotTransmissions robot_transmissions_; transmission_interface::ActuatorToJointStateInterface* act_to_jnt_state_{}; diff --git a/rm_hw/include/rm_hw/hardware_interface/types.h b/rm_hw/include/rm_hw/hardware_interface/types.h index 6eeb1f67..8dd96940 100644 --- a/rm_hw/include/rm_hw/hardware_interface/types.h +++ b/rm_hw/include/rm_hw/hardware_interface/types.h @@ -39,6 +39,7 @@ #include #include +#include #include namespace rm_hw @@ -76,6 +77,7 @@ struct ImuData double ori_cov[9], angular_vel_cov[9], linear_acc_cov[9]; double temperature, angular_vel_coeff, accel_coeff, temp_coeff, temp_offset; bool accel_updated, gyro_updated, camera_trigger; + rm_common::ImuFilterBase* imu_filter; }; struct TofData diff --git a/rm_hw/package.xml b/rm_hw/package.xml index fdf55fd5..767ac0ad 100644 --- a/rm_hw/package.xml +++ b/rm_hw/package.xml @@ -1,7 +1,7 @@ rm_hw - 0.1.8 + 0.1.9 ROS control warped interface for RoboMaster motor and some robot hardware Qiayuan Liao diff --git a/rm_hw/src/hardware_interface/can_bus.cpp b/rm_hw/src/hardware_interface/can_bus.cpp index 2245797a..d4ed70a3 100644 --- a/rm_hw/src/hardware_interface/can_bus.cpp +++ b/rm_hw/src/hardware_interface/can_bus.cpp @@ -234,6 +234,9 @@ void CanBus::read(ros::Time time) imu_data.linear_acc[1] = ((int16_t)((frame.data[3]) << 8) | frame.data[2]) * imu_data.accel_coeff; imu_data.linear_acc[2] = ((int16_t)((frame.data[5]) << 8) | frame.data[4]) * imu_data.accel_coeff; imu_data.camera_trigger = frame.data[6]; + imu_data.imu_filter->update(frame_stamp.stamp, imu_data.linear_acc, imu_data.angular_vel, imu_data.ori, + imu_data.linear_acc_cov, imu_data.angular_vel_cov, imu_data.ori_cov, + imu_data.temperature, imu_data.camera_trigger); continue; } else if (data_ptr_.id2tof_data_->find(frame.can_id) != data_ptr_.id2tof_data_->end()) diff --git a/rm_hw/src/hardware_interface/parse.cpp b/rm_hw/src/hardware_interface/parse.cpp index f4201a09..d0b5629b 100644 --- a/rm_hw/src/hardware_interface/parse.cpp +++ b/rm_hw/src/hardware_interface/parse.cpp @@ -38,6 +38,7 @@ #include "rm_hw/hardware_interface/hardware_interface.h" #include +#include #include #include #include @@ -241,69 +242,87 @@ bool rm_hw::RmRobotHW::parseImuData(XmlRpc::XmlRpcValue& imu_datas, ros::NodeHan { for (auto it = imu_datas.begin(); it != imu_datas.end(); ++it) { + std::string name = it->first; if (!it->second.hasMember("frame_id")) { - ROS_ERROR_STREAM("Imu " << it->first << " has no associated frame id."); + ROS_ERROR_STREAM("Imu " << name << " has no associated frame id."); continue; } else if (!it->second.hasMember("bus")) { - ROS_ERROR_STREAM("Imu " << it->first << " has no associated bus."); + ROS_ERROR_STREAM("Imu " << name << " has no associated bus."); continue; } else if (!it->second.hasMember("id")) { - ROS_ERROR_STREAM("Imu " << it->first << " has no associated ID."); + ROS_ERROR_STREAM("Imu " << name << " has no associated ID."); continue; } else if (!it->second.hasMember("orientation_covariance_diagonal")) { - ROS_ERROR_STREAM("Imu " << it->first << " has no associated orientation covariance diagonal."); + ROS_ERROR_STREAM("Imu " << name << " has no associated orientation covariance diagonal."); continue; } else if (!it->second.hasMember("angular_velocity_covariance")) { - ROS_ERROR_STREAM("Imu " << it->first << " has no associated angular velocity covariance."); + ROS_ERROR_STREAM("Imu " << name << " has no associated angular velocity covariance."); continue; } else if (!it->second.hasMember("linear_acceleration_covariance")) { - ROS_ERROR_STREAM("Imu " << it->first << " has no associated linear acceleration covariance."); + ROS_ERROR_STREAM("Imu " << name << " has no associated linear acceleration covariance."); continue; } else if (!it->second.hasMember("angular_vel_coeff")) { - ROS_ERROR_STREAM("Imu " << it->first << " has no associated angular velocity coefficient."); + ROS_ERROR_STREAM("Imu " << name << " has no associated angular velocity coefficient."); continue; } else if (!it->second.hasMember("accel_coeff")) { - ROS_ERROR_STREAM("Imu " << it->first << " has no associated linear acceleration coefficient."); + ROS_ERROR_STREAM("Imu " << name << " has no associated linear acceleration coefficient."); continue; } else if (!it->second.hasMember("temp_coeff")) { - ROS_ERROR_STREAM("Imu " << it->first << " has no associated temperate coefficient."); + ROS_ERROR_STREAM("Imu " << name << " has no associated temperate coefficient."); continue; } - XmlRpc::XmlRpcValue ori_cov = imu_datas[it->first]["orientation_covariance_diagonal"]; + else if (!it->second.hasMember("filter")) + { + ROS_ERROR_STREAM("Imu " << name << " has no associated filter type"); + continue; + } + XmlRpc::XmlRpcValue ori_cov = imu_datas[name]["orientation_covariance_diagonal"]; ROS_ASSERT(ori_cov.getType() == XmlRpc::XmlRpcValue::TypeArray); ROS_ASSERT(ori_cov.size() == 3); for (int i = 0; i < ori_cov.size(); ++i) ROS_ASSERT(ori_cov[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); - XmlRpc::XmlRpcValue angular_cov = imu_datas[it->first]["angular_velocity_covariance"]; + XmlRpc::XmlRpcValue angular_cov = imu_datas[name]["angular_velocity_covariance"]; ROS_ASSERT(angular_cov.getType() == XmlRpc::XmlRpcValue::TypeArray); ROS_ASSERT(angular_cov.size() == 3); for (int i = 0; i < angular_cov.size(); ++i) ROS_ASSERT(angular_cov[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); - XmlRpc::XmlRpcValue linear_cov = imu_datas[it->first]["linear_acceleration_covariance"]; + XmlRpc::XmlRpcValue linear_cov = imu_datas[name]["linear_acceleration_covariance"]; ROS_ASSERT(linear_cov.getType() == XmlRpc::XmlRpcValue::TypeArray); ROS_ASSERT(linear_cov.size() == 3); for (int i = 0; i < linear_cov.size(); ++i) ROS_ASSERT(linear_cov[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); - std::string frame_id = imu_datas[it->first]["frame_id"], bus = imu_datas[it->first]["bus"]; - int id = static_cast(imu_datas[it->first]["id"]); + std::string filter_type = imu_datas[name]["filter"]; + // TODO(Zhenyu Ye): Add more types of filter. + rm_common::ImuFilterBase* imu_filter; + if (filter_type.find("complementary") != std::string::npos) + imu_filter = new rm_common::ImuComplementaryFilter; + else + { + ROS_ERROR_STREAM("Imu " << name << " doesn't has filter type " << filter_type); + return false; + } + imu_filter->init(it->second, name); + + std::string frame_id = imu_datas[name]["frame_id"], bus = imu_datas[name]["bus"]; + int id = static_cast(imu_datas[name]["id"]); // for bus interface if (bus_id2imu_data_.find(bus) == bus_id2imu_data_.end()) @@ -328,28 +347,22 @@ bool rm_hw::RmRobotHW::parseImuData(XmlRpc::XmlRpcValue& imu_datas, ros::NodeHan static_cast(linear_cov[1]), 0., 0., 0., static_cast(linear_cov[2]) }, .temperature = 0.0, - .angular_vel_coeff = xmlRpcGetDouble(imu_datas[it->first], "angular_vel_coeff", 0.), - .accel_coeff = xmlRpcGetDouble(imu_datas[it->first], "accel_coeff", 0.), - .temp_coeff = xmlRpcGetDouble(imu_datas[it->first], "temp_coeff", 0.), - .temp_offset = xmlRpcGetDouble(imu_datas[it->first], "temp_offset", 0.), + .angular_vel_coeff = xmlRpcGetDouble(imu_datas[name], "angular_vel_coeff", 0.), + .accel_coeff = xmlRpcGetDouble(imu_datas[name], "accel_coeff", 0.), + .temp_coeff = xmlRpcGetDouble(imu_datas[name], "temp_coeff", 0.), + .temp_offset = xmlRpcGetDouble(imu_datas[name], "temp_offset", 0.), .accel_updated = false, .gyro_updated = false, - .camera_trigger = false })); + .camera_trigger = false, + .imu_filter = imu_filter })); // for ros_control interface hardware_interface::ImuSensorHandle imu_sensor_handle( - it->first, frame_id, bus_id2imu_data_[bus][id].ori, bus_id2imu_data_[bus][id].ori_cov, + name, frame_id, bus_id2imu_data_[bus][id].ori, bus_id2imu_data_[bus][id].ori_cov, bus_id2imu_data_[bus][id].angular_vel, bus_id2imu_data_[bus][id].angular_vel_cov, bus_id2imu_data_[bus][id].linear_acc, bus_id2imu_data_[bus][id].linear_acc_cov); imu_sensor_interface_.registerHandle(imu_sensor_handle); - rm_control::ImuExtraHandle imu_extra_handle(it->first, bus_id2imu_data_[bus][id].ori, - &bus_id2imu_data_[bus][id].accel_updated, - &bus_id2imu_data_[bus][id].gyro_updated, - &bus_id2imu_data_[bus][id].camera_trigger, - &bus_id2imu_data_[bus][id].temperature); - imu_extra_interface_.registerHandle(imu_extra_handle); } registerInterface(&imu_sensor_interface_); - registerInterface(&imu_extra_interface_); } catch (XmlRpc::XmlRpcException& e) { diff --git a/rm_msgs/CHANGELOG.rst b/rm_msgs/CHANGELOG.rst index 0655ac84..f85f12d4 100644 --- a/rm_msgs/CHANGELOG.rst +++ b/rm_msgs/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package rm_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.9 (2022-3-28) +------------------ +* Update rm_msgs cmake minimum required. (`#36 `_) +* Merge pull request `#27 `_ from Zhouzhenjie/master + Add the service for the conversation between a camera and a imu. +* Merge pull request `#29 `_ from Edwinlinks/tof_sensor_interface + Completed tof_sensor_interface +* Modified the reference order of header files and packet parsing of tof sensor, data type of dis_status +* Add tof sensor interface in rm_common, add parsing can frame in can_bus.cpp, and add TofSensor.msg in rm_msgs. +* Merge remote-tracking branch 'origin/master' +* Add the service for the conversation between a camera and a imu. +* Contributors: Edwinlinks, Jie j, QiayuanLiao, YuuinIH + 0.1.8 (2021-12-7) ------------------ * Merge branch 'master' into gimbal/opti_or_simplify diff --git a/rm_msgs/package.xml b/rm_msgs/package.xml index 62e17029..db5759af 100644 --- a/rm_msgs/package.xml +++ b/rm_msgs/package.xml @@ -1,7 +1,7 @@ rm_msgs - 0.1.8 + 0.1.9 The rm_msgs package provides all the messages for all kind of robot qiayuan BSD