rm_robot_hw_sim.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2  * BSD 3-Clause License
3  *
4  * Copyright (c) 2021, Qiayuan Liao
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this
11  * list of conditions and the following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice,
14  * this list of conditions and the following disclaimer in the documentation
15  * and/or other materials provided with the distribution.
16  *
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE
25  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  *******************************************************************************/
33 
34 //
35 // Created by qiayuan on 2/10/21.
36 //
37 
39 
41 
42 namespace rm_gazebo
43 {
44 bool RmRobotHWSim::initSim(const std::string& robot_namespace, ros::NodeHandle model_nh,
45  gazebo::physics::ModelPtr parent_model, const urdf::Model* urdf_model,
46  std::vector<transmission_interface::TransmissionInfo> transmissions)
47 {
48  bool ret = DefaultRobotHWSim::initSim(robot_namespace, model_nh, parent_model, urdf_model, transmissions);
52  XmlRpc::XmlRpcValue xml_rpc_value;
53 
54  if (!model_nh.getParam("imus", xml_rpc_value))
55  ROS_WARN("No imu specified");
56  else
57  parseImu(xml_rpc_value, parent_model);
58  world_ = parent_model->GetWorld(); // For gravity
59  switch_imu_service_ = model_nh.advertiseService("switch_imu_status", switchImuStatus);
60  return ret;
61 }
62 
64 {
66  if (!disable_imu_)
67  {
68  for (auto& imu : imu_datas_)
69  {
70  // TODO(qiayuan) Add noise
71  ignition::math::Pose3d pose = imu.link_ptr->WorldPose();
72  imu.time_stamp = time;
73  imu.ori[0] = pose.Rot().X();
74  imu.ori[1] = pose.Rot().Y();
75  imu.ori[2] = pose.Rot().Z();
76  imu.ori[3] = pose.Rot().W();
77  ignition::math::Vector3d rate = imu.link_ptr->RelativeAngularVel();
78  imu.angular_vel[0] = rate.X();
79  imu.angular_vel[1] = rate.Y();
80  imu.angular_vel[2] = rate.Z();
81 
82  ignition::math::Vector3d gravity = { 0., 0., -9.81 };
83  ignition::math::Vector3d accel = imu.link_ptr->RelativeLinearAccel() - pose.Rot().RotateVectorReverse(gravity);
84  imu.linear_acc[0] = accel.X();
85  imu.linear_acc[1] = accel.Y();
86  imu.linear_acc[2] = accel.Z();
87  }
88  }
89 
90  // Set cmd to zero to avoid crazy soft limit oscillation when not controller loaded
91  for (auto& cmd : joint_effort_command_)
92  cmd = 0;
93  for (auto& cmd : joint_velocity_command_)
94  cmd = 0;
95 }
96 
97 void RmRobotHWSim::parseImu(XmlRpc::XmlRpcValue& imu_datas, const gazebo::physics::ModelPtr& parent_model)
98 {
100  for (auto it = imu_datas.begin(); it != imu_datas.end(); ++it)
101  {
102  if (!it->second.hasMember("frame_id"))
103  {
104  ROS_ERROR_STREAM("Imu " << it->first << " has no associated frame id.");
105  continue;
106  }
107  else if (!it->second.hasMember("orientation_covariance_diagonal"))
108  {
109  ROS_ERROR_STREAM("Imu " << it->first << " has no associated orientation covariance diagonal.");
110  continue;
111  }
112  else if (!it->second.hasMember("angular_velocity_covariance"))
113  {
114  ROS_ERROR_STREAM("Imu " << it->first << " has no associated angular velocity covariance.");
115  continue;
116  }
117  else if (!it->second.hasMember("linear_acceleration_covariance"))
118  {
119  ROS_ERROR_STREAM("Imu " << it->first << " has no associated linear acceleration covariance.");
120  continue;
121  }
122  std::string frame_id = imu_datas[it->first]["frame_id"];
123  gazebo::physics::LinkPtr link_ptr = parent_model->GetLink(frame_id);
124  if (link_ptr == nullptr)
125  {
126  ROS_WARN("Imu %s is not specified in urdf.", it->first.c_str());
127  continue;
128  }
129  XmlRpc::XmlRpcValue ori_cov = imu_datas[it->first]["orientation_covariance_diagonal"];
131  ROS_ASSERT(ori_cov.size() == 3);
132  for (int i = 0; i < ori_cov.size(); ++i)
133  ROS_ASSERT(ori_cov[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
134  XmlRpc::XmlRpcValue angular_cov = imu_datas[it->first]["orientation_covariance_diagonal"];
136  ROS_ASSERT(angular_cov.size() == 3);
137  for (int i = 0; i < angular_cov.size(); ++i)
138  ROS_ASSERT(angular_cov[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
139  XmlRpc::XmlRpcValue linear_cov = imu_datas[it->first]["linear_acceleration_covariance"];
141  ROS_ASSERT(linear_cov.size() == 3);
142  for (int i = 0; i < linear_cov.size(); ++i)
143  ROS_ASSERT(linear_cov[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
144 
145  imu_datas_.push_back((ImuData{
146  .link_ptr = link_ptr,
147  .time_stamp = {},
148  .ori = { 0., 0., 0., 0. },
149  .ori_cov = { static_cast<double>(ori_cov[0]), 0., 0., 0., static_cast<double>(ori_cov[1]), 0., 0., 0.,
150  static_cast<double>(ori_cov[2]) },
151  .angular_vel = { 0., 0., 0. },
152  .angular_vel_cov = { static_cast<double>(angular_cov[0]), 0., 0., 0., static_cast<double>(angular_cov[1]), 0.,
153  0., 0., static_cast<double>(angular_cov[2]) },
154  .linear_acc = { 0., 0., 0. },
155  .linear_acc_cov = { static_cast<double>(linear_cov[0]), 0., 0., 0., static_cast<double>(linear_cov[1]), 0., 0.,
156  0., static_cast<double>(linear_cov[2]) } }));
157  ImuData& imu_data = imu_datas_.back();
158  hardware_interface::ImuSensorHandle imu_sensor_handle(it->first, frame_id, imu_data.ori, imu_data.ori_cov,
159  imu_data.angular_vel, imu_data.angular_vel_cov,
160  imu_data.linear_acc, imu_data.linear_acc_cov);
162  rm_imu_sensor_interface_.registerHandle(rm_control::RmImuSensorHandle(imu_sensor_handle, &imu_data.time_stamp));
163  }
164 }
165 
166 bool RmRobotHWSim::disable_imu_ = false;
167 } // namespace rm_gazebo
168 
XmlRpc::XmlRpcValue::size
int size() const
GZ_REGISTER_MODEL_PLUGIN
GZ_REGISTER_MODEL_PLUGIN(GazeboRosControlPlugin)
rm_gazebo::ImuData
Definition: rm_robot_hw_sim.h:78
gazebo_ros_control_plugin.h
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
rm_gazebo::RmRobotHWSim::imu_sensor_interface_
hardware_interface::ImuSensorInterface imu_sensor_interface_
Definition: rm_robot_hw_sim.h:110
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
hardware_interface::InterfaceManager::registerInterface
void registerInterface(T *iface)
gazebo_ros_control::DefaultRobotHWSim::readSim
virtual void readSim(ros::Time time, ros::Duration period)
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
urdf::Model
gazebo_ros_control::RobotHWSim
rm_robot_hw_sim.h
rm_gazebo::RmRobotHWSim::robot_state_interface_
rm_control::RobotStateInterface robot_state_interface_
Definition: rm_robot_hw_sim.h:109
XmlRpc::XmlRpcValue::TypeStruct
TypeStruct
rm_gazebo::RmRobotHWSim::disable_imu_
static bool disable_imu_
Definition: rm_robot_hw_sim.h:115
rm_gazebo::RmRobotHWSim::switch_imu_service_
ros::ServiceServer switch_imu_service_
Definition: rm_robot_hw_sim.h:114
XmlRpc::XmlRpcValue::TypeDouble
TypeDouble
rm_control::RmImuSensorHandle
rm_gazebo::RmRobotHWSim::readSim
void readSim(ros::Time time, ros::Duration period) override
Definition: rm_robot_hw_sim.cpp:94
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
rm_gazebo::ImuData::link_ptr
gazebo::physics::LinkPtr link_ptr
Definition: rm_robot_hw_sim.h:111
gazebo_ros_control::GazeboRosControlPlugin
rm_gazebo::RmRobotHWSim::switchImuStatus
static bool switchImuStatus(std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
Definition: rm_robot_hw_sim.h:101
gazebo_ros_control::DefaultRobotHWSim::joint_effort_command_
std::vector< double > joint_effort_command_
rm_gazebo::RmRobotHWSim::parseImu
void parseImu(XmlRpc::XmlRpcValue &imu_datas, const gazebo::physics::ModelPtr &parent_model)
Definition: rm_robot_hw_sim.cpp:128
ROS_WARN
#define ROS_WARN(...)
rm_gazebo::RmRobotHWSim::initSim
bool initSim(const std::string &robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model, const urdf::Model *urdf_model, std::vector< transmission_interface::TransmissionInfo > transmissions) override
Definition: rm_robot_hw_sim.cpp:75
rm_gazebo::RmRobotHWSim::world_
gazebo::physics::WorldPtr world_
Definition: rm_robot_hw_sim.h:112
rm_gazebo::RmRobotHWSim::rm_imu_sensor_interface_
rm_control::RmImuSensorInterface rm_imu_sensor_interface_
Definition: rm_robot_hw_sim.h:111
ResourceManager< ImuSensorHandle >::registerHandle
void registerHandle(const ImuSensorHandle &handle)
XmlRpc::XmlRpcValue::end
iterator end()
XmlRpc::XmlRpcValue::getType
const Type & getType() const
XmlRpc::XmlRpcValue::TypeArray
TypeArray
hardware_interface::ImuSensorHandle
ros::Time
rm_gazebo
Definition: rm_robot_hw_sim.h:45
rm_gazebo::RmRobotHWSim::imu_datas_
std::list< ImuData > imu_datas_
Definition: rm_robot_hw_sim.h:113
rm_gazebo::RmRobotHWSim
Definition: rm_robot_hw_sim.h:90
XmlRpc::XmlRpcValue::begin
iterator begin()
cmd
string cmd
ROS_ASSERT
#define ROS_ASSERT(cond)
gazebo_ros_control::DefaultRobotHWSim::joint_velocity_command_
std::vector< double > joint_velocity_command_
ros::Duration
XmlRpc::XmlRpcValue
ros::NodeHandle


rm_gazebo
Author(s): Qiayuan Liao
autogenerated on Tue May 6 2025 02:23:42