45 gazebo::physics::ModelPtr parent_model,
const urdf::Model* urdf_model,
46 std::vector<transmission_interface::TransmissionInfo> transmissions)
48 bool ret = DefaultRobotHWSim::initSim(robot_namespace, model_nh, parent_model, urdf_model, transmissions);
54 if (!model_nh.
getParam(
"imus", xml_rpc_value))
57 parseImu(xml_rpc_value, parent_model);
58 world_ = parent_model->GetWorld();
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();
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();
100 for (
auto it = imu_datas.
begin(); it != imu_datas.
end(); ++it)
102 if (!it->second.hasMember(
"frame_id"))
107 else if (!it->second.hasMember(
"orientation_covariance_diagonal"))
109 ROS_ERROR_STREAM(
"Imu " << it->first <<
" has no associated orientation covariance diagonal.");
112 else if (!it->second.hasMember(
"angular_velocity_covariance"))
114 ROS_ERROR_STREAM(
"Imu " << it->first <<
" has no associated angular velocity covariance.");
117 else if (!it->second.hasMember(
"linear_acceleration_covariance"))
119 ROS_ERROR_STREAM(
"Imu " << it->first <<
" has no associated linear acceleration covariance.");
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)
126 ROS_WARN(
"Imu %s is not specified in urdf.", it->first.c_str());
132 for (
int i = 0; i < ori_cov.
size(); ++i)
137 for (
int i = 0; i < angular_cov.
size(); ++i)
142 for (
int i = 0; i < linear_cov.
size(); ++i)
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]) } }));
159 imu_data.angular_vel, imu_data.angular_vel_cov,
160 imu_data.linear_acc, imu_data.linear_acc_cov);