35 #include <std_msgs/Float64.h> 36 #include <sensor_msgs/Imu.h> 37 #include <sensor_msgs/MagneticField.h> 72 publish_topic(sensor_id);
77 printf(
"OnAttributeChange(id %d, %s, %s)\n", sensor_id, attribute_name.c_str(), value.c_str());
87 nh_priv_.
setParam(
"baud_rate", baud_rate);
89 nh_priv_.
param(
"frame_id", frame_id_, std::string(
"imu_link"));
91 nh_priv_.
param(
"parent_frame_id_", parent_frame_id_, std::string(
"base_link"));
93 nh_priv_.
param(
"linear_acceleration_stddev", linear_acceleration_stddev_, 0.026831);
94 nh_priv_.
param(
"angular_velocity_stddev", angular_velocity_stddev_, 0.002428);
95 nh_priv_.
param(
"magnetic_field_stddev", magnetic_field_stddev_, 0.00000327486);
96 nh_priv_.
param(
"orientation_stddev", orientation_stddev_, 0.002143);
100 imu_mag_pub_ = nh_.
advertise<sensor_msgs::MagneticField>(
"imu/mag", 1);
101 imu_temperature_pub_= nh_.
advertise<std_msgs::Float64>(
"imu/temperature", 1);
113 if(start() ==
false)
break;
116 if(cmd_binary_data_format(
"EULER, IMU") ==
false)
break;
118 if(cmd_divider(
"1") ==
false)
break;
120 if(cmd_mode(
"BC") ==
false)
break;
143 sensor_msgs::MagneticField imu_magnetic_msg;
144 std_msgs::Float64 imu_temperature_msg;
146 double linear_acceleration_cov = linear_acceleration_stddev_ * linear_acceleration_stddev_;
147 double angular_velocity_cov = angular_velocity_stddev_ * angular_velocity_stddev_;
148 double magnetic_field_cov = magnetic_field_stddev_ * magnetic_field_stddev_;
149 double orientation_cov = orientation_stddev_ * orientation_stddev_;
151 imu_data_raw_msg.linear_acceleration_covariance[0] =
152 imu_data_raw_msg.linear_acceleration_covariance[4] =
153 imu_data_raw_msg.linear_acceleration_covariance[8] =
154 imu_data_msg.linear_acceleration_covariance[0] =
155 imu_data_msg.linear_acceleration_covariance[4] =
156 imu_data_msg.linear_acceleration_covariance[8] = linear_acceleration_cov;
158 imu_data_raw_msg.angular_velocity_covariance[0] =
159 imu_data_raw_msg.angular_velocity_covariance[4] =
160 imu_data_raw_msg.angular_velocity_covariance[8] =
161 imu_data_msg.angular_velocity_covariance[0] =
162 imu_data_msg.angular_velocity_covariance[4] =
163 imu_data_msg.angular_velocity_covariance[8] = angular_velocity_cov;
165 imu_data_msg.orientation_covariance[0] =
166 imu_data_msg.orientation_covariance[4] =
167 imu_data_msg.orientation_covariance[8] = orientation_cov;
169 imu_magnetic_msg.magnetic_field_covariance[0] =
170 imu_magnetic_msg.magnetic_field_covariance[4] =
171 imu_magnetic_msg.magnetic_field_covariance[8] = magnetic_field_cov;
173 static double convertor_g2a = 9.80665;
174 static double convertor_d2r = M_PI/180.0;
175 static double convertor_r2d = 180.0/M_PI;
176 static double convertor_ut2t = 1000000;
177 static double convertor_c = 1.0;
179 double roll, pitch, yaw;
194 imu_data_raw_msg.header.stamp =
195 imu_data_msg.header.stamp =
196 imu_magnetic_msg.header.stamp = now;
198 imu_data_raw_msg.header.frame_id =
199 imu_data_msg.header.frame_id =
200 imu_magnetic_msg.header.frame_id = frame_id_;
203 imu_data_msg.orientation.x = orientation[0];
204 imu_data_msg.orientation.y = orientation[1];
205 imu_data_msg.orientation.z = orientation[2];
206 imu_data_msg.orientation.w = orientation[3];
209 imu_data_raw_msg.linear_acceleration.x =
210 imu_data_msg.linear_acceleration.x = imu.
ax * convertor_g2a;
211 imu_data_raw_msg.linear_acceleration.y =
212 imu_data_msg.linear_acceleration.y = -imu.
ay * convertor_g2a;
213 imu_data_raw_msg.linear_acceleration.z =
214 imu_data_msg.linear_acceleration.z = -imu.
az * convertor_g2a;
217 imu_data_raw_msg.angular_velocity.x =
218 imu_data_msg.angular_velocity.x = imu.
gx * convertor_d2r;
219 imu_data_raw_msg.angular_velocity.y =
220 imu_data_msg.angular_velocity.y = -imu.
gy * convertor_d2r;
221 imu_data_raw_msg.angular_velocity.z =
222 imu_data_msg.angular_velocity.z = -imu.
gz * convertor_d2r;
225 imu_magnetic_msg.magnetic_field.x = imu.
mx / convertor_ut2t;
226 imu_magnetic_msg.magnetic_field.y = -imu.
my / convertor_ut2t;
227 imu_magnetic_msg.magnetic_field.z = -imu.
mz / convertor_ut2t;
233 imu_data_raw_pub_.
publish(imu_data_raw_msg);
234 imu_data_pub_.
publish(imu_data_msg);
235 imu_mag_pub_.
publish(imu_magnetic_msg);
236 imu_temperature_pub_.
publish(imu_temperature_msg);
247 int main(
int argc,
char* argv[])
251 std::string port = std::string(
"/dev/ttyACM0");
252 int baud_rate = 115200;
261 ROS_ERROR(
"%s\n",
"Initialize() returns false, please check your devices.");
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
ros::Publisher imu_data_pub_
void OnSensorData(int sensor_id, SensorData data)
std::string parent_frame_id_
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf::TransformBroadcaster broadcaster_
ros::Publisher imu_temperature_pub_
MyAhrsDriverForROS(std::string port="", int baud_rate=115200)
ros::Publisher imu_mag_pub_
ROSCPP_DECL void spin(Spinner &spinner)
void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value)
void publish_topic(int sensor_id)
ROSCPP_DECL bool get(const std::string &key, std::string &s)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
double magnetic_field_stddev_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
double linear_acceleration_stddev_
int main(int argc, char *argv[])
double orientation_stddev_
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
void get_data(SensorData &data)
double angular_velocity_stddev_
ros::Publisher imu_data_raw_pub_