rclcpp type of support for transparent porting

[bobd988]

The goal is to expose same type of rclcpp type of APIs to application developer. Here is how it works from application code

sensor_msgs::msg::Imu imu_msg;
std::string imu_frame_id;

void receive_CAN(const can_msgs::msg::Frame::ConstSharedPtr msg)
{
  if (msg->id == 0x319) {
    imu_msg.header.frame_id = imu_frame_id;
    imu_msg.header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();

    counter = msg->data[1] + (msg->data[0] << 8);
    angular_velocity_x_raw = msg->data[3] + (msg->data[2] << 8);
    imu_msg.angular_velocity.x =
      angular_velocity_x_raw * (200 / pow(2, 15)) * M_PI / 180;  // LSB & unit [deg/s] => [rad/s]
    angular_velocity_y_raw = msg->data[5] + (msg->data[4] << 8);
    imu_msg.angular_velocity.y =
      angular_velocity_y_raw * (200 / pow(2, 15)) * M_PI / 180;  // LSB & unit [deg/s] => [rad/s]
    angular_velocity_z_raw = msg->data[7] + (msg->data[6] << 8);
    imu_msg.angular_velocity.z =
      angular_velocity_z_raw * (200 / pow(2, 15)) * M_PI / 180;  // LSB & unit [deg/s] => [rad/s]
  }
  if (msg->id == 0x31A) {
    acceleration_x_raw = msg->data[3] + (msg->data[2] << 8);
    imu_msg.linear_acceleration.x = acceleration_x_raw * (100 / pow(2, 15));  // LSB & unit [m/s^2]
    acceleration_y_raw = msg->data[5] + (msg->data[4] << 8);
    imu_msg.linear_acceleration.y = acceleration_y_raw * (100 / pow(2, 15));  // LSB & unit [m/s^2]
    acceleration_z_raw = msg->data[7] + (msg->data[6] << 8);
    imu_msg.linear_acceleration.z = acceleration_z_raw * (100 / pow(2, 15));  // LSB & unit [m/s^2]

    imu_msg.orientation.x = 0.0;
    imu_msg.orientation.y = 0.0;
    imu_msg.orientation.z = 0.0;
    imu_msg.orientation.w = 1.0;
    pub->publish(imu_msg);
  }
}

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);

  auto node = rclcpp::Node::make_shared("tag_can_driver");
  imu_frame_id = node->declare_parameter<std::string>("imu_frame_id", "imu");
  rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr sub = node->create_subscription<can_msgs::msg::Frame>("/can/imu", 100, receive_CAN);
  pub = node->create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 100);
  rclcpp::spin(node);

  return 0;
}

The methods in main are targeted for ROS2 node. However the above methods can also be targeted for DORA node with minimal changes from the syntax.