19 #include <gazebo/sensors/ImuSensor.hh> 20 #include <gazebo/physics/World.hh> 26 accelerometer_data = ignition::math::Vector3d(0, 0, 0);
27 gyroscope_data = ignition::math::Vector3d(0, 0, 0);
28 orientation = ignition::math::Quaterniond(1,0,0,0);
36 sensor=
dynamic_cast<gazebo::sensors::ImuSensor*
>(sensor_.get());
40 ROS_FATAL(
"Error: Sensor pointer is NULL!");
54 ROS_FATAL(
"ROS has not been initialized!");
69 common::Time current_time =
sensor->LastUpdateTime();
96 imu_msg.orientation_covariance[0] = gn2;
97 imu_msg.orientation_covariance[4] = gn2;
98 imu_msg.orientation_covariance[8] = gn2;
99 imu_msg.angular_velocity_covariance[0] = gn2;
100 imu_msg.angular_velocity_covariance[4] = gn2;
101 imu_msg.angular_velocity_covariance[8] = gn2;
102 imu_msg.linear_acceleration_covariance[0] = gn2;
103 imu_msg.linear_acceleration_covariance[4] = gn2;
104 imu_msg.linear_acceleration_covariance[8] = gn2;
108 imu_msg.header.stamp.sec = current_time.sec;
109 imu_msg.header.stamp.nsec = current_time.nsec;
123 double U1 =
static_cast<double>(rand_r(&
seed)) / static_cast<double>(RAND_MAX);
124 double U2 =
static_cast<double>(rand_r(&
seed)) / static_cast<double>(RAND_MAX);
127 double Z0 = sqrt(-2.0 * ::log(U1)) * cos(2.0*M_PI * U2);
130 Z0 = sigma * Z0 + mu;
139 if (
sdf->HasElement(
"robotNamespace"))
146 std::string scoped_name =
sensor->ParentName();
147 std::size_t it = scoped_name.find(
"::");
154 if (
sdf->HasElement(
"topicName"))
166 if (
sdf->HasElement(
"frameName"))
173 ROS_FATAL(
"missing <frameName>, cannot proceed");
178 if (
sdf->HasElement(
"updateRateHZ"))
190 if (
sdf->HasElement(
"gaussianNoise"))
202 if (
sdf->HasElement(
"xyzOffset"))
204 offset.Pos() =
sdf->Get<ignition::math::Vector3d>(
"xyzOffset");
209 offset.Pos() = ignition::math::Vector3d(0, 0, 0);
214 if (
sdf->HasElement(
"rpyOffset"))
216 offset.Rot() = ignition::math::Quaterniond(
sdf->Get<ignition::math::Vector3d>(
"rpyOffset"));
221 offset.Rot() = ignition::math::Quaterniond::Identity;
double update_rate
Sensor update rate.
sdf::ElementPtr sdf
Pointer to the sdf config file.
void publish(const boost::shared_ptr< M > &message) const
std::string robot_namespace
The data is published on the topic named: /robot_namespace/topic_name.
ROSCPP_DECL bool isInitialized()
double GuassianKernel(double mu, double sigma)
Gaussian noise generator.
ros::Publisher imu_data_publisher
Ros Publisher for imu data.
Gazebo Ros imu sensor plugin.
double gaussian_noise
Gaussian noise.
ignition::math::Vector3d accelerometer_data
Linear acceleration data from the sensor.
ignition::math::Vector3d gyroscope_data
Angular velocity data from the sensor.
ros::NodeHandle * node
Ros NodeHandle pointer.
sensors::ImuSensor * sensor
Pointer to the sensor.
virtual void Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_)
Load the sensor.
unsigned int seed
Seed for the Gaussian noise generator.
ignition::math::Quaterniond orientation
Orientation data from the sensor.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void UpdateChild(const gazebo::common::UpdateInfo &)
Update the sensor.
#define ROS_WARN_STREAM(args)
virtual ~GazeboRosImuSensor()
Destructor.
bool LoadParameters()
Load the parameters from the sdf file.
common::Time last_time
last time on which the data was published.
sensor_msgs::Imu imu_msg
Ros IMU message.
uint32_t getNumSubscribers() const
#define ROS_INFO_STREAM(args)
gazebo::event::ConnectionPtr connection
Pointer to the update event connection.
std::string topic_name
The data is published on the topic named: /robot_namespace/topic_name.
ROSCPP_DECL void spinOnce()
ignition::math::Pose3d offset
Offset parameter, position part is unused.
std::string body_name
Name of the link of the IMU.