Skip to content

Commit

Permalink
Add output in robot frame option (#26)
Browse files Browse the repository at this point in the history 
* Correctly convert quaternion to ROS frame

* Rename tf_ned_to_enu to tf_ned_to_nwu

* Cleanup

* Update to handle two options

* lint

* Change indentation

* Fix formatting issues + add namespace to enum
  • Loading branch information
@bhomberg
bhomberg authored Feb 10, 2020
1 parent e2c6f25 commit 466e6e0
Showing 1 changed file with 160 additions and 66 deletions.
226 changes: 160 additions & 66 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,15 @@ const char VERSION[10] = "0.0.2"; // um7_driver version
// us to publish everything we have.
const uint8_t TRIGGER_PACKET = DREG_EULER_PHI_THETA;

namespace OutputAxisOptions
{
enum OutputAxisOption
{
DEFAULT, ENU, ROBOT_FRAME
};
}
typedef OutputAxisOptions::OutputAxisOption OutputAxisOption;

/**
* Function generalizes the process of writing an XYZ vector into consecutive
* fields in UM7 registers.
Expand Down Expand Up @@ -206,7 +215,7 @@ bool handleResetService(um7::Comms* sensor,
* the ROS messages which are output.
*/
void publishMsgs(um7::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& imu_msg,
bool tf_ned_to_enu, bool use_magnetic_field_msg)
OutputAxisOption axes, bool use_magnetic_field_msg)
{
static ros::Publisher imu_pub = imu_nh->advertise<sensor_msgs::Imu>("data", 1, false);
static ros::Publisher mag_pub;
Expand All @@ -223,40 +232,66 @@ void publishMsgs(um7::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& i

if (imu_pub.getNumSubscribers() > 0)
{
// body-fixed frame NED to ENU: (x y z)->(x -y -z) or (w x y z)->(x -y -z w)
// world frame NED to ENU: (x y z)->(y x -z) or (w x y z)->(y x -z w)
if (tf_ned_to_enu)
switch (axes)
{
// world frame
imu_msg.orientation.w = r.quat.get_scaled(2);
imu_msg.orientation.x = r.quat.get_scaled(1);
imu_msg.orientation.y = -r.quat.get_scaled(3);
imu_msg.orientation.z = r.quat.get_scaled(0);

// body-fixed frame
imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
imu_msg.angular_velocity.y = -r.gyro.get_scaled(1);
imu_msg.angular_velocity.z = -r.gyro.get_scaled(2);

// body-fixed frame
imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
imu_msg.linear_acceleration.y = -r.accel.get_scaled(1);
imu_msg.linear_acceleration.z = -r.accel.get_scaled(2);
}
else
{
imu_msg.orientation.w = r.quat.get_scaled(0);
imu_msg.orientation.x = r.quat.get_scaled(1);
imu_msg.orientation.y = r.quat.get_scaled(2);
imu_msg.orientation.z = r.quat.get_scaled(3);

imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
imu_msg.angular_velocity.y = r.gyro.get_scaled(1);
imu_msg.angular_velocity.z = r.gyro.get_scaled(2);

imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
imu_msg.linear_acceleration.y = r.accel.get_scaled(1);
imu_msg.linear_acceleration.z = r.accel.get_scaled(2);
case OutputAxisOptions::ENU:
{
// body-fixed frame NED to ENU: (x y z)->(x -y -z) or (w x y z)->(x -y -z w)
// world frame NED to ENU: (x y z)->(y x -z) or (w x y z)->(y x -z w)
// world frame
imu_msg.orientation.w = r.quat.get_scaled(2);
imu_msg.orientation.x = r.quat.get_scaled(1);
imu_msg.orientation.y = -r.quat.get_scaled(3);
imu_msg.orientation.z = r.quat.get_scaled(0);

// body-fixed frame
imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
imu_msg.angular_velocity.y = -r.gyro.get_scaled(1);
imu_msg.angular_velocity.z = -r.gyro.get_scaled(2);

// body-fixed frame
imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
imu_msg.linear_acceleration.y = -r.accel.get_scaled(1);
imu_msg.linear_acceleration.z = -r.accel.get_scaled(2);
break;
}
case OutputAxisOptions::ROBOT_FRAME:
{
// body-fixed frame
imu_msg.orientation.w = -r.quat.get_scaled(0);
imu_msg.orientation.x = -r.quat.get_scaled(1);
imu_msg.orientation.y = r.quat.get_scaled(2);
imu_msg.orientation.z = r.quat.get_scaled(3);

// body-fixed frame
imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
imu_msg.angular_velocity.y = -r.gyro.get_scaled(1);
imu_msg.angular_velocity.z = -r.gyro.get_scaled(2);

// body-fixed frame
imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
imu_msg.linear_acceleration.y = -r.accel.get_scaled(1);
imu_msg.linear_acceleration.z = -r.accel.get_scaled(2);
break;
}
case OutputAxisOptions::DEFAULT:
{
imu_msg.orientation.w = r.quat.get_scaled(0);
imu_msg.orientation.x = r.quat.get_scaled(1);
imu_msg.orientation.y = r.quat.get_scaled(2);
imu_msg.orientation.z = r.quat.get_scaled(3);

imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
imu_msg.angular_velocity.y = r.gyro.get_scaled(1);
imu_msg.angular_velocity.z = r.gyro.get_scaled(2);

imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
imu_msg.linear_acceleration.y = r.accel.get_scaled(1);
imu_msg.linear_acceleration.z = r.accel.get_scaled(2);
break;
}
default:
ROS_ERROR("OuputAxes enum value invalid");
}

imu_pub.publish(imu_msg);
Expand All @@ -270,17 +305,32 @@ void publishMsgs(um7::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& i
sensor_msgs::MagneticField mag_msg;
mag_msg.header = imu_msg.header;

if (tf_ned_to_enu)
{
mag_msg.magnetic_field.x = r.mag.get_scaled(1);
mag_msg.magnetic_field.y = r.mag.get_scaled(0);
mag_msg.magnetic_field.z = -r.mag.get_scaled(2);
}
else
switch (axes)
{
mag_msg.magnetic_field.x = r.mag.get_scaled(0);
mag_msg.magnetic_field.y = r.mag.get_scaled(1);
mag_msg.magnetic_field.z = r.mag.get_scaled(2);
case OutputAxisOptions::ENU:
{
mag_msg.magnetic_field.x = r.mag.get_scaled(1);
mag_msg.magnetic_field.y = r.mag.get_scaled(0);
mag_msg.magnetic_field.z = -r.mag.get_scaled(2);
break;
}
case OutputAxisOptions::ROBOT_FRAME:
{
// body-fixed frame
mag_msg.magnetic_field.x = r.mag.get_scaled(0);
mag_msg.magnetic_field.y = -r.mag.get_scaled(1);
mag_msg.magnetic_field.z = -r.mag.get_scaled(2);
break;
}
case OutputAxisOptions::DEFAULT:
{
mag_msg.magnetic_field.x = r.mag.get_scaled(0);
mag_msg.magnetic_field.y = r.mag.get_scaled(1);
mag_msg.magnetic_field.z = r.mag.get_scaled(2);
break;
}
default:
ROS_ERROR("OuputAxes enum value invalid");
}

mag_pub.publish(mag_msg);
Expand All @@ -290,17 +340,32 @@ void publishMsgs(um7::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& i
geometry_msgs::Vector3Stamped mag_msg;
mag_msg.header = imu_msg.header;

if (tf_ned_to_enu)
{
mag_msg.vector.x = r.mag.get_scaled(1);
mag_msg.vector.y = r.mag.get_scaled(0);
mag_msg.vector.z = -r.mag.get_scaled(2);
}
else
switch (axes)
{
mag_msg.vector.x = r.mag.get_scaled(0);
mag_msg.vector.y = r.mag.get_scaled(1);
mag_msg.vector.z = r.mag.get_scaled(2);
case OutputAxisOptions::ENU:
{
mag_msg.vector.x = r.mag.get_scaled(1);
mag_msg.vector.y = r.mag.get_scaled(0);
mag_msg.vector.z = -r.mag.get_scaled(2);
break;
}
case OutputAxisOptions::ROBOT_FRAME:
{
// body-fixed frame
mag_msg.vector.x = r.mag.get_scaled(0);
mag_msg.vector.y = -r.mag.get_scaled(1);
mag_msg.vector.z = -r.mag.get_scaled(2);
break;
}
case OutputAxisOptions::DEFAULT:
{
mag_msg.vector.x = r.mag.get_scaled(0);
mag_msg.vector.y = r.mag.get_scaled(1);
mag_msg.vector.z = r.mag.get_scaled(2);
break;
}
default:
ROS_ERROR("OuputAxes enum value invalid");
}

mag_pub.publish(mag_msg);
Expand All @@ -313,18 +378,32 @@ void publishMsgs(um7::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& i
geometry_msgs::Vector3Stamped rpy_msg;
rpy_msg.header = imu_msg.header;

if (tf_ned_to_enu)
switch (axes)
{
// world frame
rpy_msg.vector.x = r.euler.get_scaled(1);
rpy_msg.vector.y = r.euler.get_scaled(0);
rpy_msg.vector.z = -r.euler.get_scaled(2);
}
else
{
rpy_msg.vector.x = r.euler.get_scaled(0);
rpy_msg.vector.y = r.euler.get_scaled(1);
rpy_msg.vector.z = r.euler.get_scaled(2);
case OutputAxisOptions::ENU:
{
// world frame
rpy_msg.vector.x = r.euler.get_scaled(1);
rpy_msg.vector.y = r.euler.get_scaled(0);
rpy_msg.vector.z = -r.euler.get_scaled(2);
break;
}
case OutputAxisOptions::ROBOT_FRAME:
{
rpy_msg.vector.x = r.euler.get_scaled(0);
rpy_msg.vector.y = -r.euler.get_scaled(1);
rpy_msg.vector.z = -r.euler.get_scaled(2);
break;
}
case OutputAxisOptions::DEFAULT:
{
rpy_msg.vector.x = r.euler.get_scaled(0);
rpy_msg.vector.y = r.euler.get_scaled(1);
rpy_msg.vector.z = r.euler.get_scaled(2);
break;
}
default:
ROS_ERROR("OuputAxes enum value invalid");
}

rpy_pub.publish(rpy_msg);
Expand Down Expand Up @@ -382,7 +461,22 @@ int main(int argc, char **argv)

// Enable converting from NED to ENU by default
bool tf_ned_to_enu;
bool orientation_in_robot_frame;
private_nh.param<bool>("tf_ned_to_enu", tf_ned_to_enu, true);
private_nh.param<bool>("orientation_in_robot_frame", orientation_in_robot_frame, false);
OutputAxisOption axes = OutputAxisOptions::DEFAULT;
if (tf_ned_to_enu && orientation_in_robot_frame)
{
ROS_ERROR("Requested IMU data in two separate frames.");
}
else if (tf_ned_to_enu)
{
axes = OutputAxisOptions::ENU;
}
else if (orientation_in_robot_frame)
{
axes = OutputAxisOptions::ROBOT_FRAME;
}

// Use MagneticField message rather than Vector3Stamped.
bool use_magnetic_field_msg;
Expand Down Expand Up @@ -432,7 +526,7 @@ int main(int argc, char **argv)
{
// Triggered by arrival of final message in group.
imu_msg.header.stamp = ros::Time::now();
publishMsgs(registers, &imu_nh, imu_msg, tf_ned_to_enu, use_magnetic_field_msg);
publishMsgs(registers, &imu_nh, imu_msg, axes, use_magnetic_field_msg);
ros::spinOnce();
}
}
Expand Down

0 comments on commit 466e6e0

Please sign in to comment.