mirrors/scout_ros
1
0
Fork 0
mirror of https://github.com/westonrobot/scout_ros.git synced 2026-01-12 11:20:02 +08:00
Code Issues Packages Projects Releases Wiki Activity

removed deprecated source

Browse Source
This commit is contained in:
Ruixiang Du
2020-04-02 11:49:52 +08:00
parent 426089c7b0
commit 46eca41492
1 changed files with 0 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
scout_base/src/imu_broadcast_node.cpp
View File

@@ -1,72 +0,0 @@
/*
* imu_broadcast_node.cpp
*
* Created on: Oct 02, 2019 19:09
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include <string>
#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/wescore.hpp"
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
ros::Publisher imu_pub;
struct MessageBroadcaster
{
void IMULCMCallback(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const wescore_lcm_msgs::IMU *msg)
{
static uint64_t count = 0;
// std::cout << "imu msg received" << std::endl;
sensor_msgs::Imu imu_msg;
imu_msg.header.frame_id = "imu_link";
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.seq = count++;
imu_msg.angular_velocity.x = msg->angular_velocity.x;
imu_msg.angular_velocity.y = msg->angular_velocity.y;
imu_msg.angular_velocity.z = msg->angular_velocity.z;
imu_msg.linear_acceleration.x = msg->linear_acceleration.x;
imu_msg.linear_acceleration.y = msg->linear_acceleration.y;
imu_msg.linear_acceleration.z = msg->linear_acceleration.z;
for (int i = 0; i < 9; ++i)
{
imu_msg.orientation_covariance[i] = msg->orientation_covariance[i];
imu_msg.angular_velocity_covariance[i] = msg->angular_velocity_covariance[i];
imu_msg.linear_acceleration_covariance[i] = msg->linear_acceleration_covariance[i];
}
imu_pub.publish(imu_msg);
}
};
int main(int argc, char **argv)
{
// setup LCM
lcm::LCM lcm;
if (!lcm.good())
return 1;
MessageBroadcaster mb;
lcm.subscribe("sensor_hub_raw_imu", &MessageBroadcaster::IMULCMCallback, &mb);
// setup ROS node
ros::init(argc, argv, "imu_broadcast_node");
ros::NodeHandle nh;
imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 1000);
ROS_INFO("Started broadcasting");
while (ros::ok())
{
lcm.handleTimeout(5);
}
return 0;
}