37 #include "geometry_msgs/Vector3Stamped.h" 39 #include "sensor_msgs/Imu.h" 40 #include "sensor_msgs/MagneticField.h" 42 #include "std_msgs/Float32.h" 43 #include "std_msgs/Header.h" 46 #include "um6/Reset.h" 56 template<
typename RegT>
58 std::string param, std::string human_name)
62 throw std::logic_error(
"configureVector3 may only be used with 3-field registers!");
72 << x <<
", " << y <<
", " << z <<
")");
78 throw std::runtime_error(
"Unable to configure vector.");
87 template<
typename RegT>
93 throw std::runtime_error(
"Command to device failed.");
107 const uint8_t UM6_BAUD_115200 = 0x5;
114 private_nh->
param<
int>(
"update_rate", rate, 20);
115 if (rate < 20 || rate > 300)
117 ROS_WARN(
"Potentially unsupported update rate of %d", rate);
120 uint32_t rate_bits = (uint32_t) ((rate-20)*255.0/280.0);
121 ROS_INFO(
"Setting update rate to %uHz", rate);
126 throw std::runtime_error(
"Unable to set communication register.");
130 bool mag_updates, accel_updates;
131 private_nh->
param<
bool>(
"mag_updates", mag_updates,
true);
132 private_nh->
param<
bool>(
"accel_updates", accel_updates,
true);
140 ROS_WARN(
"Excluding magnetometer updates from EKF.");
148 ROS_WARN(
"Excluding accelerometer updates from EKF.");
153 throw std::runtime_error(
"Unable to set misc config register.");
160 private_nh->
param<
bool>(
"zero_gyros", zero_gyros,
true);
173 const um6::Reset::Request& req,
const um6::Reset::Response& resp)
188 bool tf_ned_to_enu,
bool use_magnetic_field_msg)
192 if (use_magnetic_field_msg)
194 mag_pub = imu_nh->
advertise<sensor_msgs::MagneticField>(
"mag", 1,
false);
198 mag_pub = imu_nh->
advertise<geometry_msgs::Vector3Stamped>(
"mag", 1,
false);
252 if (mag_pub.getNumSubscribers() > 0)
254 if (use_magnetic_field_msg)
256 sensor_msgs::MagneticField mag_msg;
257 mag_msg.header = imu_msg.header;
272 mag_pub.publish(mag_msg);
276 geometry_msgs::Vector3Stamped mag_msg;
277 mag_msg.header = imu_msg.header;
292 mag_pub.publish(mag_msg);
296 if (rpy_pub.getNumSubscribers() > 0)
298 geometry_msgs::Vector3Stamped rpy_msg;
299 rpy_msg.header = imu_msg.header;
314 rpy_pub.publish(rpy_msg);
317 if (temp_pub.getNumSubscribers() > 0)
319 std_msgs::Float32 temp_msg;
321 temp_pub.publish(temp_msg);
328 int main(
int argc,
char **argv)
337 private_nh.
param<std::string>(
"port", port,
"/dev/ttyUSB0");
338 private_nh.
param<int32_t>(
"baud", baud, 115200);
342 ser.setBaudrate(baud);
346 sensor_msgs::Imu imu_msg;
347 double linear_acceleration_stdev, angular_velocity_stdev;
348 private_nh.
param<std::string>(
"frame_id", imu_msg.header.frame_id,
"imu_link");
350 private_nh.
param<
double>(
"linear_acceleration_stdev", linear_acceleration_stdev, 0.06);
351 private_nh.
param<
double>(
"angular_velocity_stdev", angular_velocity_stdev, 0.005);
353 double linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev;
354 double angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev;
358 private_nh.
param<
bool>(
"tf_ned_to_enu", tf_ned_to_enu,
true);
361 bool use_magnetic_field_msg;
362 private_nh.
param<
bool>(
"use_magnetic_field_msg", use_magnetic_field_msg,
false);
364 imu_msg.linear_acceleration_covariance[0] = linear_acceleration_cov;
365 imu_msg.linear_acceleration_covariance[4] = linear_acceleration_cov;
366 imu_msg.linear_acceleration_covariance[8] = linear_acceleration_cov;
368 imu_msg.angular_velocity_covariance[0] = angular_velocity_cov;
369 imu_msg.angular_velocity_covariance[4] = angular_velocity_cov;
370 imu_msg.angular_velocity_covariance[8] = angular_velocity_cov;
372 bool first_failure =
true;
385 ROS_INFO(
"Successfully connected to serial port.");
386 first_failure =
true;
392 ros::ServiceServer srv = imu_nh.advertiseService<um6::Reset::Request, um6::Reset::Response>(
401 publishMsgs(registers, &imu_nh, imu_msg, tf_ned_to_enu, use_magnetic_field_msg);
406 catch(
const std::exception& e)
408 if (ser.isOpen()) ser.close();
410 ROS_INFO(
"Attempting reconnection after error.");
417 << port <<
". Trying again every 1 second.");
418 first_failure =
false;
const Accessor< int16_t > quat
#define UM6_EULER_ENABLED
#define UM6_GYROS_PROC_ENABLED
const Accessor< int16_t > accel
void publish(const boost::shared_ptr< M > &message) const
#define UM6_SERIAL_RATE_MASK
#define UM6_ACCEL_UPDATE_ENABLED
double get_scaled(uint16_t field) const
#define UM6_BROADCAST_ENABLED
void configureSensor(um6::Comms *sensor, ros::NodeHandle *private_nh)
int main(int argc, char **argv)
const Accessor< int16_t > gyro
const Accessor< float > mag_ref
Provides the Registers class, which initializes with a suite of accessors suitable for reading and wr...
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define UM6_BAUD_START_BIT
void configureVector3(um6::Comms *sensor, const um6::Accessor< RegT > ®, std::string param, std::string human_name)
#define UM6_MAG_UPDATE_ENABLED
const Accessor< float > covariance
const Accessor< int16_t > gyro_bias
#define UM6_ACCELS_PROC_ENABLED
const uint8_t TRIGGER_PACKET
const Accessor< float > temperature
ROSCPP_DECL bool get(const std::string &key, std::string &s)
const Accessor< int16_t > mag
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
const Accessor< uint32_t > communication
int16_t receive(Registers *r)
const Accessor< int16_t > mag_bias
#define ROS_WARN_STREAM_COND(cond, args)
ROSCPP_DECL bool has(const std::string &key)
void set_scaled(uint16_t field, double value) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const Accessor< int16_t > accel_bias
bool sendWaitAck(const Accessor_ &a)
const Accessor< uint32_t > cmd_set_accel_ref
uint32_t getNumSubscribers() const
void setPort(const std::string &port)
void sendCommand(um6::Comms *sensor, const um6::Accessor< RegT > ®, std::string human_name)
#define ROS_INFO_STREAM(args)
const Accessor< uint32_t > cmd_reset_ekf
const Accessor< uint32_t > cmd_set_mag_ref
Comms class definition. Does not manage the serial connection itself, but takes care of reading and w...
const Accessor< uint32_t > cmd_zero_gyros
#define UM6_TEMPERATURE_ENABLED
const Accessor< float > accel_ref
bool handleResetService(um6::Comms *sensor, const um6::Reset::Request &req, const um6::Reset::Response &resp)
#define UM6_MAG_PROC_ENABLED
const Accessor< int16_t > euler
#define ROS_ERROR_STREAM(args)
ROSCPP_DECL void spinOnce()
#define UM6_QUAT_ESTIMATE_ENABLED
void set(uint8_t field, RegT value) const
void publishMsgs(um6::Registers &r, ros::NodeHandle *imu_nh, sensor_msgs::Imu &imu_msg, bool tf_ned_to_enu, bool use_magnetic_field_msg)
const Accessor< uint32_t > misc_config