houyongdi/UnmannedShip
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 14:50:01 +08:00
commit 36193aec44
1006 changed files with 163575 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/.gitignore
+34
View File
@@ -0,0 +1,34 @@
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
# Temporary files
*.user
.idea/
cmake-build-debug/
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/CMakeLists.txt
+90
View File
@@ -0,0 +1,90 @@
cmake_minimum_required(VERSION 2.8.3)
project(lpms_ig1)
set(CMAKE_CXX_FLAGS "-std=c++11")
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation
)
generate_messages(
DEPENDENCIES
std_msgs
)
link_directories("${IG1_LIB}")
set(lpms_ig1_node_SRCS
src/lpms_ig1_node.cpp
)
set(lpms_ig1_rs485_node_SRCS
src/lpms_ig1_rs485_node.cpp
)
set(lpms_ig1_rs485_client_SRCS
src/lpms_ig1_rs485_client.cpp
)
set(lpms_be1_node_SRCS
src/lpms_be1_node.cpp
)
set(lpms_nav3_node_SRCS
src/lpms_nav3_node.cpp
)
## Declare a catkin package
catkin_package()
## Build
include_directories(include ${catkin_INCLUDE_DIRS})
# lpms_ig1_node
add_executable(lpms_ig1_node ${lpms_ig1_node_SRCS})
target_link_libraries(lpms_ig1_node
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_ig1_node ${catkin_EXPORTED_TARGETS})
# lpms_ig1_rs485_node
add_executable(lpms_ig1_rs485_node ${lpms_ig1_rs485_node_SRCS})
target_link_libraries(lpms_ig1_rs485_node
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_ig1_rs485_node ${catkin_EXPORTED_TARGETS})
# lpms_ig1_rs485_client
add_executable(lpms_ig1_rs485_client ${lpms_ig1_rs485_client_SRCS})
target_link_libraries(lpms_ig1_rs485_client
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_ig1_rs485_client ${catkin_EXPORTED_TARGETS})
# lpms_be1_node
add_executable(lpms_be1_node ${lpms_be1_node_SRCS})
target_link_libraries(lpms_be1_node
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_be1_node ${catkin_EXPORTED_TARGETS})
# lpms_NAV3_node
add_executable(lpms_nav3_node ${lpms_nav3_node_SRCS})
target_link_libraries(lpms_nav3_node
${catkin_LIBRARIES}
LpmsIG1_OpenSourceLib.so
)
add_dependencies(lpms_nav3_node ${catkin_EXPORTED_TARGETS})
# imudata_rad_to_deg_node
add_executable(imudata_rad_to_deg_node src/imudata_rad_to_deg_node.cpp)
target_link_libraries(imudata_rad_to_deg_node ${catkin_LIBRARIES})
add_dependencies(imudata_rad_to_deg_node ${catkin_EXPORTED_TARGETS})
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/launch/lpmsbe1.launch
+18
View File
@@ -0,0 +1,18 @@
<launch>
<!-- IG1 Sensor node -->
<node name="lpms_be1" pkg="lpms_ig1" type="lpms_be1_node" output="screen">
<param name="port" value="0001" type="string" />
<param name="baudrate" value="115200" type="int" />
<param name="frame_id" value="imu" type="string" />
</node>
<!-- imudata rad to deg conversion node -->
<node name="imudata_deg" pkg="lpms_ig1" type="imudata_rad_to_deg_node" />
<!-- Plots -->
<node name="plot_imu_gyro" pkg="rqt_plot" type="rqt_plot"
args="/angular_vel_deg" />
<node name="plot_imu_euler" pkg="rqt_plot" type="rqt_plot"
args="/rpy_deg" />
</launch>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/launch/lpmsig1.launch
+18
View File
@@ -0,0 +1,18 @@
<launch>
<!-- IG1 Sensor node -->
<node name="lpms_ig1" pkg="lpms_ig1" type="lpms_ig1_node">
<param name="port" value="/dev/ttyUSB0" type="string" />
<param name="baudrate" value="921600" type="int" />
<param name="frame_id" value="imu" type="string" />
</node>
<!-- imudata rad to deg conversion node -->
<node name="imudata_deg" pkg="lpms_ig1" type="imudata_rad_to_deg_node" />
<!-- Plots -->
<node name="plot_imu_gyro" pkg="rqt_plot" type="rqt_plot"
args="/angular_vel_deg" />
<node name="plot_imu_euler" pkg="rqt_plot" type="rqt_plot"
args="/rpy_deg" />
</launch>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/launch/lpmsig1_rs485.launch
+20
View File
@@ -0,0 +1,20 @@
<launch>
<!-- IG1 RS484 Sensor node -->
<node name="lpms_ig1_rs485" pkg="lpms_ig1" type="lpms_ig1_rs485_node">
<param name="port" value="/dev/ttyTHS5" type="string" />
<param name="baudrate" value="115200" type="int" />
<param name="rs485ControlPin" value="388" type="int" />
<param name="rs485ControlPinToggleWaitMs" value="2" type="int" />
<param name="frame_id" value="imu" type="string" />
</node>
<!-- imudata rad to deg conversion node -->
<node name="imudata_deg" pkg="lpms_ig1" type="imudata_rad_to_deg_node" />
<!-- Plots -->
<node name="plot_imu_gyro" pkg="rqt_plot" type="rqt_plot"
args="/angular_vel_deg" />
<node name="plot_imu_euler" pkg="rqt_plot" type="rqt_plot"
args="/rpy_deg" />
</launch>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/launch/lpmsnav3.launch
+19
View File
@@ -0,0 +1,19 @@
<launch>
<!-- IG1 Sensor node -->
<node name="lpms_nav3" pkg="lpms_ig1" type="lpms_nav3_node" output="screen">
<param name="port" value="/dev/ttyUSB0" type="string" />
<param name="baudrate" value="115200" type="int" />
<param name="frame_id" value="imu" type="string" />
</node>
<!-- imudata rad to deg conversion node -->
<node name="imudata_deg" pkg="lpms_ig1" type="imudata_rad_to_deg_node" />
<!-- Plots -->
<node name="plot_imu_gyro" pkg="rqt_plot" type="rqt_plot"
args="/angular_vel_deg" />
<node name="plot_imu_euler" pkg="rqt_plot" type="rqt_plot"
args="/rpy_deg" />
</launch>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/package.xml
+52
View File
@@ -0,0 +1,52 @@
<?xml version="1.0"?>
<package>
<name>lpms_ig1</name>
<version>0.0.1</version>
<description>ROS driver for lpms_ig1 sensors.</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="lxf@alubi.cn">Feng</maintainer>
<maintainer email="yap@lp-research.com">H.E. YAP</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="xxxxx">xxxxx</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<!-- build_depend>orocos_kdl</build_depend -->
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<!-- run_depend>orocos_kdl</run_depend -->
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/imudata_rad_to_deg_node.cpp
+47
View File
@@ -0,0 +1,47 @@
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <iostream>
#include <tf/transform_datatypes.h>
ros::Publisher angular_vel_deg_publisher;
ros::Publisher rpy_deg_publisher;
ros::Subscriber imudata_subscriber;
const float r2d = 57.29577951f;
void MsgCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
geometry_msgs::Vector3 angular_vel;
angular_vel.x = msg->angular_velocity.x*r2d;
angular_vel.y = msg->angular_velocity.y*r2d;
angular_vel.z = msg->angular_velocity.z*r2d;
angular_vel_deg_publisher.publish(angular_vel);
tf::Quaternion q(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
geometry_msgs::Vector3 rpy;
rpy.x = roll*r2d;
rpy.y = pitch*r2d;
rpy.z = yaw*r2d;
rpy_deg_publisher.publish(rpy);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_listener");
ros::NodeHandle n;
angular_vel_deg_publisher = n.advertise<geometry_msgs::Vector3>("angular_vel_deg", 1000);
rpy_deg_publisher = n.advertise<geometry_msgs::Vector3>("rpy_deg", 1000);
imudata_subscriber = n.subscribe("/imu/data", 1000, MsgCallback);
ROS_INFO("waiting for imu data");
ros::spin();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_be1_node.cpp
+399
View File
@@ -0,0 +1,399 @@
#include <string>
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Trigger.h"
#include "std_msgs/Bool.h"
#include "lpsensor/LpmsIG1I.h"
#include "lpsensor/SensorDataI.h"
#include "lpsensor/LpmsIG1Registers.h"
//! Manages connection with the sensor, publishes data
/*!
\TODO: Make noncopyable!
*/
struct IG1Command
{
short command;
union Data {
uint32_t i[64];
float f[64];
unsigned char c[256];
} data;
int dataLength;
};
class LpBE1Proxy
{
public:
// Node handler
ros::NodeHandle nh, private_nh;
ros::Timer updateTimer;
// Publisher
ros::Publisher imu_pub;
ros::Publisher autocalibration_status_pub;
// Service
ros::ServiceServer autocalibration_serv;
ros::ServiceServer autoReconnect_serv;
ros::ServiceServer gyrocalibration_serv;
ros::ServiceServer resetHeading_serv;
ros::ServiceServer getImuData_serv;
ros::ServiceServer setStreamingMode_serv;
ros::ServiceServer setCommandMode_serv;
sensor_msgs::Imu imu_msg;
// Parameters
std::string comportNo;
int baudrate;
bool autoReconnect;
std::string frame_id;
int rate;
LpBE1Proxy(ros::NodeHandle h) :
nh(h),
private_nh("~")
{
// Get node parameters
private_nh.param<std::string>("port", comportNo, "/dev/ttyUSB0");
private_nh.param("baudrate", baudrate, 115200);
private_nh.param("autoreconnect", autoReconnect, true);
private_nh.param<std::string>("frame_id", frame_id, "imu");
private_nh.param("rate", rate, 200);
// Create LpmsBE1 object
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(autoReconnect);
imu_pub = nh.advertise<sensor_msgs::Imu>("data",1);
autocalibration_status_pub = nh.advertise<std_msgs::Bool>("is_autocalibration_active", 1, true);
autocalibration_serv = nh.advertiseService("enable_gyro_autocalibration", &LpBE1Proxy::setAutocalibration, this);
autoReconnect_serv = nh.advertiseService("enable_auto_reconnect", &LpBE1Proxy::setAutoReconnect, this);
gyrocalibration_serv = nh.advertiseService("calibrate_gyroscope", &LpBE1Proxy::calibrateGyroscope, this);
resetHeading_serv = nh.advertiseService("reset_heading", &LpBE1Proxy::resetHeading, this);
getImuData_serv = nh.advertiseService("get_imu_data", &LpBE1Proxy::getImuData, this);
setStreamingMode_serv = nh.advertiseService("set_streaming_mode", &LpBE1Proxy::setStreamingMode, this);
setCommandMode_serv = nh.advertiseService("set_command_mode", &LpBE1Proxy::setCommandMode, this);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
//logd(TAG, "Error connecting to sensor\n");
ROS_ERROR("Error connecting to sensor\n");
sensor1->release();
ros::Duration(3).sleep(); // sleep 3 s
}
do
{
ROS_INFO("Waiting for sensor to connect %d", sensor1->getStatus());
ros::Duration(1).sleep();
} while(
ros::ok() &&
(
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
)
);
if (sensor1->getStatus() == STATUS_CONNECTED)
{
ROS_INFO("Sensor connected");
ros::Duration(1).sleep();
sensor1->commandGotoStreamingMode();
}
else
{
ROS_INFO("Sensor connection error: %d.", sensor1->getStatus());
ros::shutdown();
}
}
~LpBE1Proxy(void)
{
sensor1->release();
}
void update(const ros::TimerEvent& te)
{
static bool runOnce = false;
if (sensor1->getStatus() == STATUS_CONNECTED &&
sensor1->hasImuData())
{
if (!runOnce)
{
publishIsAutocalibrationActive();
runOnce = true;
}
IG1ImuDataI sd;
sensor1->getImuData(sd);
/* Fill the IMU message */
// Fill the header
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
// Fill orientation quaternion
imu_msg.orientation.w = sd.quaternion.data[0];
imu_msg.orientation.x = -sd.quaternion.data[1];
imu_msg.orientation.y = -sd.quaternion.data[2];
imu_msg.orientation.z = -sd.quaternion.data[3];
// Fill angular velocity data
// - scale from deg/s to rad/s
imu_msg.angular_velocity.x = sd.gyroIIAlignmentCalibrated.data[0]*3.1415926/180;
imu_msg.angular_velocity.y = sd.gyroIIAlignmentCalibrated.data[1]*3.1415926/180;
imu_msg.angular_velocity.z = sd.gyroIIAlignmentCalibrated.data[2]*3.1415926/180;
// Fill linear acceleration data
imu_msg.linear_acceleration.x = -sd.accCalibrated.data[0]*9.81;
imu_msg.linear_acceleration.y = -sd.accCalibrated.data[1]*9.81;
imu_msg.linear_acceleration.z = -sd.accCalibrated.data[2]*9.81;
// Publish the messages
imu_pub.publish(imu_msg);
}
}
void run(void)
{
// The timer ensures periodic data publishing
updateTimer = ros::Timer(nh.createTimer(ros::Duration(1.0f/rate),
&LpBE1Proxy::update,
this));
}
void publishIsAutocalibrationActive()
{
std_msgs::Bool msg;
IG1SettingsI settings;
sensor1->getSettings(settings);
msg.data = settings.enableGyroAutocalibration;
autocalibration_status_pub.publish(msg);
}
///////////////////////////////////////////////////
// Service Callbacks
///////////////////////////////////////////////////
bool setAutocalibration (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_autocalibration");
// clear current settings
IG1SettingsI settings;
sensor1->getSettings(settings);
// Send command
cmdSetEnableAutocalibration(req.data);
ros::Duration(0.2).sleep();
cmdGetEnableAutocalibration();
ros::Duration(0.1).sleep();
double retryElapsedTime = 0;
int retryCount = 0;
while (!sensor1->hasSettings())
{
ros::Duration(0.1).sleep();
ROS_INFO("set_autocalibration wait");
retryElapsedTime += 0.1;
if (retryElapsedTime > 2.0)
{
retryElapsedTime = 0;
cmdGetEnableAutocalibration();
retryCount++;
}
if (retryCount > 5)
break;
}
ROS_INFO("set_autocalibration done");
// Get settings
sensor1->getSettings(settings);
std::string msg;
if (settings.enableGyroAutocalibration == req.data)
{
res.success = true;
msg.append(std::string("[Success] autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
else
{
res.success = false;
msg.append(std::string("[Failed] current autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
ROS_INFO("%s", msg.c_str());
res.message = msg;
publishIsAutocalibrationActive();
return res.success;
}
// Auto reconnect
bool setAutoReconnect (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_auto_reconnect");
sensor1->setAutoReconnectStatus(req.data);
res.success = true;
std::string msg;
msg.append(std::string("[Success] auto reconnection status set to: ") + (sensor1->getAutoReconnectStatus()?"True":"False"));
ROS_INFO("%s", msg.c_str());
res.message = msg;
return res.success;
}
// reset heading
bool resetHeading (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("reset_heading");
// Send command
cmdResetHeading();
res.success = true;
res.message = "[Success] Heading reset";
return true;
}
bool calibrateGyroscope (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("calibrate_gyroscope: Please make sure the sensor is stationary for 4 seconds");
cmdCalibrateGyroscope();
ros::Duration(4).sleep();
res.success = true;
res.message = "[Success] Gyroscope calibration procedure completed";
ROS_INFO("calibrate_gyroscope: Gyroscope calibration procedure completed");
return true;
}
bool getImuData (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
ros::Duration(0.1).sleep();
cmdGetImuData();
res.success = true;
res.message = "[Success] Get imu data";
return true;
}
bool setStreamingMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoStreamingMode();
res.success = true;
res.message = "[Success] Set streaming mode";
return true;
}
bool setCommandMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
res.success = true;
res.message = "[Success] Set command mode";
return true;
}
///////////////////////////////////////////////////
// Helpers
///////////////////////////////////////////////////
void cmdGotoCommandMode ()
{
IG1Command cmd;
cmd.command = GOTO_COMMAND_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGotoStreamingMode ()
{
IG1Command cmd;
cmd.command = GOTO_STREAM_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetImuData()
{
IG1Command cmd;
cmd.command = GET_IMU_DATA;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdCalibrateGyroscope()
{
IG1Command cmd;
cmd.command = START_GYR_CALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdResetHeading()
{
IG1Command cmd;
cmd.command = SET_ORIENTATION_OFFSET;
cmd.dataLength = 4;
cmd.data.i[0] = LPMS_OFFSET_MODE_HEADING;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdSetEnableAutocalibration(int status)
{
IG1Command cmd;
cmd.command = SET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 4;
cmd.data.i[0] = status;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetEnableAutocalibration()
{
IG1Command cmd;
cmd.command = GET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
private:
// Access to LPMS data
IG1I* sensor1;
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "lpms_be1_node");
ros::NodeHandle nh("imu");
ros::AsyncSpinner spinner(0);
spinner.start();
LpBE1Proxy lpBE1(nh);
lpBE1.run();
ros::waitForShutdown();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_ig1_node.cpp
+410
View File
@@ -0,0 +1,410 @@
#include <string>
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Trigger.h"
#include "std_msgs/Bool.h"
#include "lpsensor/LpmsIG1I.h"
#include "lpsensor/SensorDataI.h"
#include "lpsensor/LpmsIG1Registers.h"
struct IG1Command
{
short command;
union Data {
uint32_t i[64];
float f[64];
unsigned char c[256];
} data;
int dataLength;
};
class LpIG1Proxy
{
public:
// Node handler
ros::NodeHandle nh, private_nh;
ros::Timer updateTimer;
// Publisher
ros::Publisher imu_pub;
ros::Publisher mag_pub;
ros::Publisher autocalibration_status_pub;
// Service
ros::ServiceServer autocalibration_serv;
ros::ServiceServer autoReconnect_serv;
ros::ServiceServer gyrocalibration_serv;
ros::ServiceServer resetHeading_serv;
ros::ServiceServer getImuData_serv;
ros::ServiceServer setStreamingMode_serv;
ros::ServiceServer setCommandMode_serv;
sensor_msgs::Imu imu_msg;
sensor_msgs::MagneticField mag_msg;
// Parameters
std::string comportNo;
int baudrate;
bool autoReconnect;
std::string frame_id;
int rate;
LpIG1Proxy(ros::NodeHandle h) :
nh(h),
private_nh("~")
{
// Get node parameters
private_nh.param<std::string>("port", comportNo, "/dev/ttyUSB0");
private_nh.param("baudrate", baudrate, 921600);
private_nh.param("autoreconnect", autoReconnect, true);
private_nh.param<std::string>("frame_id", frame_id, "imu");
private_nh.param("rate", rate, 200);
// Create LpmsIG1 object
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(autoReconnect);
ROS_INFO("Settings");
ROS_INFO("Port: %s", comportNo.c_str());
ROS_INFO("Baudrate: %d", baudrate);
ROS_INFO("Auto reconnect: %s", autoReconnect? "Enabled":"Disabled");
imu_pub = nh.advertise<sensor_msgs::Imu>("data",1);
mag_pub = nh.advertise<sensor_msgs::MagneticField>("mag",1);
autocalibration_status_pub = nh.advertise<std_msgs::Bool>("is_autocalibration_active", 1, true);
autocalibration_serv = nh.advertiseService("enable_gyro_autocalibration", &LpIG1Proxy::setAutocalibration, this);
autoReconnect_serv = nh.advertiseService("enable_auto_reconnect", &LpIG1Proxy::setAutoReconnect, this);
gyrocalibration_serv = nh.advertiseService("calibrate_gyroscope", &LpIG1Proxy::calibrateGyroscope, this);
resetHeading_serv = nh.advertiseService("reset_heading", &LpIG1Proxy::resetHeading, this);
getImuData_serv = nh.advertiseService("get_imu_data", &LpIG1Proxy::getImuData, this);
setStreamingMode_serv = nh.advertiseService("set_streaming_mode", &LpIG1Proxy::setStreamingMode, this);
setCommandMode_serv = nh.advertiseService("set_command_mode", &LpIG1Proxy::setCommandMode, this);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
ROS_ERROR("Error connecting to sensor\n");
sensor1->release();
ros::Duration(3).sleep(); // sleep 3 s
}
do
{
ROS_INFO("Waiting for sensor to connect %d", sensor1->getStatus());
ros::Duration(1).sleep();
} while(
ros::ok() &&
(
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
)
);
if (sensor1->getStatus() == STATUS_CONNECTED)
{
ROS_INFO("Sensor connected");
ros::Duration(1).sleep();
sensor1->commandGotoStreamingMode();
}
else
{
ROS_INFO("Sensor connection error: %d.", sensor1->getStatus());
ros::shutdown();
}
}
~LpIG1Proxy(void)
{
sensor1->release();
}
void update(const ros::TimerEvent& te)
{
static bool runOnce = false;
if (sensor1->getStatus() == STATUS_CONNECTED &&
sensor1->hasImuData())
{
if (!runOnce)
{
publishIsAutocalibrationActive();
runOnce = true;
}
IG1ImuDataI sd;
sensor1->getImuData(sd);
/* Fill the IMU message */
// Fill the header
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
// Fill orientation quaternion
imu_msg.orientation.w = sd.quaternion.data[0];
imu_msg.orientation.x = -sd.quaternion.data[1];
imu_msg.orientation.y = -sd.quaternion.data[2];
imu_msg.orientation.z = -sd.quaternion.data[3];
// Fill angular velocity data
// - scale from deg/s to rad/s
imu_msg.angular_velocity.x = sd.gyroIAlignmentCalibrated.data[0]*3.1415926/180;
imu_msg.angular_velocity.y = sd.gyroIAlignmentCalibrated.data[1]*3.1415926/180;
imu_msg.angular_velocity.z = sd.gyroIAlignmentCalibrated.data[2]*3.1415926/180;
// Fill linear acceleration data
imu_msg.linear_acceleration.x = -sd.accCalibrated.data[0]*9.81;
imu_msg.linear_acceleration.y = -sd.accCalibrated.data[1]*9.81;
imu_msg.linear_acceleration.z = -sd.accCalibrated.data[2]*9.81;
/* Fill the magnetometer message */
mag_msg.header.stamp = imu_msg.header.stamp;
mag_msg.header.frame_id = frame_id;
// Units are microTesla in the LPMS library, Tesla in ROS.
mag_msg.magnetic_field.x = sd.magRaw.data[0]*1e-6;
mag_msg.magnetic_field.y = sd.magRaw.data[1]*1e-6;
mag_msg.magnetic_field.z = sd.magRaw.data[2]*1e-6;
// Publish the messages
imu_pub.publish(imu_msg);
mag_pub.publish(mag_msg);
}
}
void run(void)
{
// The timer ensures periodic data publishing
updateTimer = ros::Timer(nh.createTimer(ros::Duration(1.0f/rate),
&LpIG1Proxy::update,
this));
}
void publishIsAutocalibrationActive()
{
std_msgs::Bool msg;
IG1SettingsI settings;
sensor1->getSettings(settings);
msg.data = settings.enableGyroAutocalibration;
autocalibration_status_pub.publish(msg);
}
///////////////////////////////////////////////////
// Service Callbacks
///////////////////////////////////////////////////
bool setAutocalibration (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_autocalibration");
// clear current settings
IG1SettingsI settings;
sensor1->getSettings(settings);
// Send command
cmdSetEnableAutocalibration(req.data);
ros::Duration(0.2).sleep();
cmdGetEnableAutocalibration();
ros::Duration(0.1).sleep();
double retryElapsedTime = 0;
int retryCount = 0;
while (!sensor1->hasSettings())
{
ros::Duration(0.1).sleep();
ROS_INFO("set_autocalibration wait");
retryElapsedTime += 0.1;
if (retryElapsedTime > 2.0)
{
retryElapsedTime = 0;
cmdGetEnableAutocalibration();
retryCount++;
}
if (retryCount > 5)
break;
}
ROS_INFO("set_autocalibration done");
// Get settings
sensor1->getSettings(settings);
std::string msg;
if (settings.enableGyroAutocalibration == req.data)
{
res.success = true;
msg.append(std::string("[Success] autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
else
{
res.success = false;
msg.append(std::string("[Failed] current autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
ROS_INFO("%s", msg.c_str());
res.message = msg;
publishIsAutocalibrationActive();
return res.success;
}
// Auto reconnect
bool setAutoReconnect (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_auto_reconnect");
sensor1->setAutoReconnectStatus(req.data);
res.success = true;
std::string msg;
msg.append(std::string("[Success] auto reconnection status set to: ") + (sensor1->getAutoReconnectStatus()?"True":"False"));
ROS_INFO("%s", msg.c_str());
res.message = msg;
return res.success;
}
// reset heading
bool resetHeading (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("reset_heading");
// Send command
cmdResetHeading();
res.success = true;
res.message = "[Success] Heading reset";
return true;
}
bool calibrateGyroscope (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("calibrate_gyroscope: Please make sure the sensor is stationary for 4 seconds");
cmdCalibrateGyroscope();
ros::Duration(4).sleep();
res.success = true;
res.message = "[Success] Gyroscope calibration procedure completed";
ROS_INFO("calibrate_gyroscope: Gyroscope calibration procedure completed");
return true;
}
bool getImuData (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
ros::Duration(0.1).sleep();
cmdGetImuData();
res.success = true;
res.message = "[Success] Get imu data";
return true;
}
bool setStreamingMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoStreamingMode();
res.success = true;
res.message = "[Success] Set streaming mode";
return true;
}
bool setCommandMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
res.success = true;
res.message = "[Success] Set command mode";
return true;
}
///////////////////////////////////////////////////
// Helpers
///////////////////////////////////////////////////
void cmdGotoCommandMode ()
{
IG1Command cmd;
cmd.command = GOTO_COMMAND_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGotoStreamingMode ()
{
IG1Command cmd;
cmd.command = GOTO_STREAM_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetImuData()
{
IG1Command cmd;
cmd.command = GET_IMU_DATA;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdCalibrateGyroscope()
{
IG1Command cmd;
cmd.command = START_GYR_CALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdResetHeading()
{
IG1Command cmd;
cmd.command = SET_ORIENTATION_OFFSET;
cmd.dataLength = 4;
cmd.data.i[0] = LPMS_OFFSET_MODE_HEADING;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdSetEnableAutocalibration(int status)
{
IG1Command cmd;
cmd.command = SET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 4;
cmd.data.i[0] = status;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetEnableAutocalibration()
{
IG1Command cmd;
cmd.command = GET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
private:
// Access to LPMS data
IG1I* sensor1;
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "lpms_ig1_node");
ros::NodeHandle nh("imu");
ros::AsyncSpinner spinner(0);
spinner.start();
LpIG1Proxy lpIG1(nh);
lpIG1.run();
ros::waitForShutdown();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_ig1_rs485_client.cpp
+36
View File
@@ -0,0 +1,36 @@
#include "ros/ros.h"
#include <std_srvs/Trigger.h>
#include <cstdlib>
int main(int argc, char **argv)
{
ros::init(argc, argv, "lpms_ig1_rs485_client");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<std_srvs::TriggerRequest, std_srvs::TriggerResponse>("/imu/get_imu_data");
ros::Rate loop_rate(50);
std_srvs::TriggerRequest req;
std_srvs::TriggerResponse res;
int count = 0;
while (ros::ok())
{
if (client.call(req, res))
{
ROS_INFO("get_imu_data: %d", count++);
}
else
{
ROS_ERROR("Failed to call service get_imu_data");
return 1;
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_ig1_rs485_node.cpp
+369
View File
@@ -0,0 +1,369 @@
#include <string>
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Trigger.h"
#include "std_msgs/Bool.h"
#include "lpsensor/LpmsIG1I.h"
#include "lpsensor/SensorDataI.h"
#include "lpsensor/LpmsIG1Registers.h"
struct IG1Command
{
short command;
union Data {
uint32_t i[64];
float f[64];
unsigned char c[256];
} data;
int dataLength;
};
class LpIG1Proxy
{
public:
// Node handler
ros::NodeHandle nh, private_nh;
ros::Timer updateTimer;
// Publisher
ros::Publisher imu_pub;
ros::Publisher mag_pub;
ros::Publisher autocalibration_status_pub;
// Service
ros::ServiceServer autocalibration_serv;
ros::ServiceServer autoReconnect_serv;
ros::ServiceServer gyrocalibration_serv;
ros::ServiceServer resetHeading_serv;
ros::ServiceServer getImuData_serv;
ros::ServiceServer setStreamingMode_serv;
ros::ServiceServer setCommandMode_serv;
sensor_msgs::Imu imu_msg;
sensor_msgs::MagneticField mag_msg;
// Parameters
std::string comportNo;
int baudrate;
int startupMode;
bool autoReconnect;
std::string frame_id;
int rs485ControlPin;
int rs485ControlPinToggleWaitMs;
int rate;
LpIG1Proxy(ros::NodeHandle h) :
nh(h),
private_nh("~")
{
// Get node parameters
private_nh.param<std::string>("port", comportNo, "/dev/ttyUSB0");
private_nh.param("baudrate", baudrate, 115200);
private_nh.param("startupmode", startupMode, SENSOR_MODE_STREAMING);
private_nh.param("autoreconnect", autoReconnect, true);
private_nh.param("rs485ControlPin", rs485ControlPin, -1);
private_nh.param("rs485ControlPinToggleWaitMs", rs485ControlPinToggleWaitMs, 2);
private_nh.param<std::string>("frame_id", frame_id, "imu");
private_nh.param("rate", rate, 200);
// Create LpmsIG1 object
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(autoReconnect);
sensor1->setStartupSensorMode(startupMode);
sensor1->setConnectionInterface(CONNECTION_INTERFACE_RS485);
sensor1->setControlGPIOForRs485(rs485ControlPin);
sensor1->setControlGPIOToggleWaitMs(rs485ControlPinToggleWaitMs);
ROS_INFO("Settings");
ROS_INFO("Port: %s", comportNo.c_str());
ROS_INFO("Baudrate: %d", baudrate);
ROS_INFO("Startup mode: %s", (startupMode == 0)? "Command mode":"Streaming mode");
ROS_INFO("Auto reconnect: %s", autoReconnect? "Enabled":"Disabled");
ROS_INFO("rs485ControlPin: %d", rs485ControlPin);
ROS_INFO("rs485ControlPinToggleWaitMs: %d", rs485ControlPinToggleWaitMs);
imu_pub = nh.advertise<sensor_msgs::Imu>("data",1);
mag_pub = nh.advertise<sensor_msgs::MagneticField>("mag",1);
autocalibration_status_pub = nh.advertise<std_msgs::Bool>("is_autocalibration_active", 1, true);
autocalibration_serv = nh.advertiseService("enable_gyro_autocalibration", &LpIG1Proxy::setAutocalibration, this);
autoReconnect_serv = nh.advertiseService("enable_auto_reconnect", &LpIG1Proxy::setAutoReconnect, this);
gyrocalibration_serv = nh.advertiseService("calibrate_gyroscope", &LpIG1Proxy::calibrateGyroscope, this);
resetHeading_serv = nh.advertiseService("reset_heading", &LpIG1Proxy::resetHeading, this);
getImuData_serv = nh.advertiseService("get_imu_data", &LpIG1Proxy::getImuData, this);
setStreamingMode_serv = nh.advertiseService("set_streaming_mode", &LpIG1Proxy::setStreamingMode, this);
setCommandMode_serv = nh.advertiseService("set_command_mode", &LpIG1Proxy::setCommandMode, this);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
ROS_ERROR("Error connecting to sensor\n");
sensor1->release();
ros::Duration(3).sleep(); // sleep 3 s
}
do
{
ROS_INFO("Waiting for sensor to connect. Sensor status: %d", sensor1->getStatus());
ros::Duration(1).sleep();
} while(
ros::ok() &&
(
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
)
);
if (sensor1->getStatus() == STATUS_CONNECTED)
{
ROS_INFO("Sensor connected");
ros::Duration(1).sleep();
//sensor1->commandGotoStreamingMode();
}
else
{
ROS_INFO("Sensor connection error: %d.", sensor1->getStatus());
ros::shutdown();
}
}
~LpIG1Proxy(void)
{
sensor1->release();
}
void update(const ros::TimerEvent& te)
{
static bool runOnce = false;
if (sensor1->getStatus() == STATUS_CONNECTED &&
sensor1->hasImuData())
{
if (!runOnce)
{
publishIsAutocalibrationActive();
runOnce = true;
}
IG1ImuDataI sd;
sensor1->getImuData(sd);
/* Fill the IMU message */
// Fill the header
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
// Fill orientation quaternion
imu_msg.orientation.w = sd.quaternion.data[0];
imu_msg.orientation.x = -sd.quaternion.data[1];
imu_msg.orientation.y = -sd.quaternion.data[2];
imu_msg.orientation.z = -sd.quaternion.data[3];
// Fill angular velocity data
// - scale from deg/s to rad/s
imu_msg.angular_velocity.x = sd.gyroIAlignmentCalibrated.data[0]*3.1415926/180;
imu_msg.angular_velocity.y = sd.gyroIAlignmentCalibrated.data[1]*3.1415926/180;
imu_msg.angular_velocity.z = sd.gyroIAlignmentCalibrated.data[2]*3.1415926/180;
// Fill linear acceleration data
imu_msg.linear_acceleration.x = -sd.accCalibrated.data[0]*9.81;
imu_msg.linear_acceleration.y = -sd.accCalibrated.data[1]*9.81;
imu_msg.linear_acceleration.z = -sd.accCalibrated.data[2]*9.81;
/* Fill the magnetometer message */
mag_msg.header.stamp = imu_msg.header.stamp;
mag_msg.header.frame_id = frame_id;
// Units are microTesla in the LPMS library, Tesla in ROS.
mag_msg.magnetic_field.x = sd.magRaw.data[0]*1e-6;
mag_msg.magnetic_field.y = sd.magRaw.data[1]*1e-6;
mag_msg.magnetic_field.z = sd.magRaw.data[2]*1e-6;
// Publish the messages
imu_pub.publish(imu_msg);
mag_pub.publish(mag_msg);
}
}
void run(void)
{
// The timer ensures periodic data publishing
updateTimer = ros::Timer(nh.createTimer(ros::Duration(1.0f/rate),
&LpIG1Proxy::update,
this));
}
void publishIsAutocalibrationActive()
{
std_msgs::Bool msg;
IG1SettingsI settings;
sensor1->getSettings(settings);
msg.data = settings.enableGyroAutocalibration;
autocalibration_status_pub.publish(msg);
}
///////////////////////////////////////////////////
// Service Callbacks
///////////////////////////////////////////////////
bool setAutocalibration (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_autocalibration");
// clear current settings
IG1SettingsI settings;
sensor1->getSettings(settings);
sensor1->commandSetGyroAutoCalibration(req.data);
ros::Duration(0.2).sleep();
double retryElapsedTime = 0;
int retryCount = 0;
while (!sensor1->hasSettings())
{
ros::Duration(0.1).sleep();
ROS_INFO("set_autocalibration wait");
retryElapsedTime += 0.1;
if (retryElapsedTime > 2.0)
{
retryElapsedTime = 0;
sensor1->commandGetGyroAutoCalibration();
retryCount++;
}
if (retryCount > 5)
break;
}
ROS_INFO("set_autocalibration done");
// Get settings
sensor1->getSettings(settings);
std::string msg;
if (settings.enableGyroAutocalibration == req.data)
{
res.success = true;
msg.append(std::string("[Success] autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
else
{
res.success = false;
msg.append(std::string("[Failed] current autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
ROS_INFO("%s", msg.c_str());
res.message = msg;
publishIsAutocalibrationActive();
return res.success;
}
// Auto reconnect
bool setAutoReconnect (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_auto_reconnect");
sensor1->setAutoReconnectStatus(req.data);
res.success = true;
std::string msg;
msg.append(std::string("[Success] auto reconnection status set to: ") + (sensor1->getAutoReconnectStatus()?"True":"False"));
ROS_INFO("%s", msg.c_str());
res.message = msg;
return res.success;
}
// reset heading
bool resetHeading (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("reset_heading");
sensor1->commandSetOffsetMode(LPMS_OFFSET_MODE_HEADING);
res.success = true;
res.message = "[Success] Heading resets";
return true;
}
bool calibrateGyroscope (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("calibrate_gyroscope: Please make sure the sensor is stationary for 4 seconds");
sensor1->commandStartGyroCalibration();
ros::Duration(4).sleep();
res.success = true;
res.message = "[Success] Gyroscope calibration procedure completed";
ROS_INFO("calibrate_gyroscope: Gyroscope calibration procedure completed");
//sensor1->commandGotoStreamingMode();
return true;
}
bool getImuData (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGetImuData();
res.success = true;
res.message = "[Success] Get imu data";
return true;
}
bool setStreamingMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
sensor1->commandGotoStreamingMode();
res.success = true;
res.message = "[Success] Set streaming mode";
return true;
}
bool setCommandMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
sensor1->commandGotoCommandMode();
res.success = true;
res.message = "[Success] Set command mode";
return true;
}
///////////////////////////////////////////////////
// Helpers
///////////////////////////////////////////////////
void cmdGetImuData()
{
IG1Command cmd;
cmd.command = GET_IMU_DATA;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
private:
// Access to LPMS data
IG1I* sensor1;
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "lpms_ig1_node_rs485");
ros::NodeHandle nh("imu");
ros::AsyncSpinner spinner(0);
spinner.start();
LpIG1Proxy lpIG1(nh);
lpIG1.run();
ros::waitForShutdown();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/lpms_nav3_node.cpp
+399
View File
@@ -0,0 +1,399 @@
#include <string>
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Trigger.h"
#include "std_msgs/Bool.h"
#include "lpsensor/LpmsIG1I.h"
#include "lpsensor/SensorDataI.h"
#include "lpsensor/LpmsIG1Registers.h"
//! Manages connection with the sensor, publishes data
/*!
\TODO: Make noncopyable!
*/
struct IG1Command
{
short command;
union Data {
uint32_t i[64];
float f[64];
unsigned char c[256];
} data;
int dataLength;
};
class LpNAV3Proxy
{
public:
// Node handler
ros::NodeHandle nh, private_nh;
ros::Timer updateTimer;
// Publisher
ros::Publisher imu_pub;
ros::Publisher autocalibration_status_pub;
// Service
ros::ServiceServer autocalibration_serv;
ros::ServiceServer autoReconnect_serv;
ros::ServiceServer gyrocalibration_serv;
ros::ServiceServer resetHeading_serv;
ros::ServiceServer getImuData_serv;
ros::ServiceServer setStreamingMode_serv;
ros::ServiceServer setCommandMode_serv;
sensor_msgs::Imu imu_msg;
// Parameters
std::string comportNo;
int baudrate;
bool autoReconnect;
std::string frame_id;
int rate;
LpNAV3Proxy(ros::NodeHandle h) :
nh(h),
private_nh("~")
{
// Get node parameters
private_nh.param<std::string>("port", comportNo, "/dev/ttyUSB0");
private_nh.param("baudrate", baudrate, 115200);
private_nh.param("autoreconnect", autoReconnect, true);
private_nh.param<std::string>("frame_id", frame_id, "imu");
private_nh.param("rate", rate, 200);
// Create LpmsNAV3 object
sensor1 = IG1Factory();
sensor1->setVerbose(VERBOSE_INFO);
sensor1->setAutoReconnectStatus(autoReconnect);
imu_pub = nh.advertise<sensor_msgs::Imu>("data",1);
autocalibration_status_pub = nh.advertise<std_msgs::Bool>("is_autocalibration_active", 1, true);
autocalibration_serv = nh.advertiseService("enable_gyro_autocalibration", &LpNAV3Proxy::setAutocalibration, this);
autoReconnect_serv = nh.advertiseService("enable_auto_reconnect", &LpNAV3Proxy::setAutoReconnect, this);
gyrocalibration_serv = nh.advertiseService("calibrate_gyroscope", &LpNAV3Proxy::calibrateGyroscope, this);
resetHeading_serv = nh.advertiseService("reset_heading", &LpNAV3Proxy::resetHeading, this);
getImuData_serv = nh.advertiseService("get_imu_data", &LpNAV3Proxy::getImuData, this);
setStreamingMode_serv = nh.advertiseService("set_streaming_mode", &LpNAV3Proxy::setStreamingMode, this);
setCommandMode_serv = nh.advertiseService("set_command_mode", &LpNAV3Proxy::setCommandMode, this);
// Connects to sensor
if (!sensor1->connect(comportNo, baudrate))
{
//logd(TAG, "Error connecting to sensor\n");
ROS_ERROR("Error connecting to sensor\n");
sensor1->release();
ros::Duration(3).sleep(); // sleep 3 s
}
do
{
ROS_INFO("Waiting for sensor to connect %d", sensor1->getStatus());
ros::Duration(1).sleep();
} while(
ros::ok() &&
(
!(sensor1->getStatus() == STATUS_CONNECTED) &&
!(sensor1->getStatus() == STATUS_CONNECTION_ERROR)
)
);
if (sensor1->getStatus() == STATUS_CONNECTED)
{
ROS_INFO("Sensor connected");
ros::Duration(1).sleep();
sensor1->commandGotoStreamingMode();
}
else
{
ROS_INFO("Sensor connection error: %d.", sensor1->getStatus());
ros::shutdown();
}
}
~LpNAV3Proxy(void)
{
sensor1->release();
}
void update(const ros::TimerEvent& te)
{
static bool runOnce = false;
if (sensor1->getStatus() == STATUS_CONNECTED &&
sensor1->hasImuData())
{
if (!runOnce)
{
publishIsAutocalibrationActive();
runOnce = true;
}
IG1ImuDataI sd;
sensor1->getImuData(sd);
/* Fill the IMU message */
// Fill the header
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
// Fill orientation quaternion
imu_msg.orientation.w = sd.quaternion.data[0];
imu_msg.orientation.x = sd.quaternion.data[1];
imu_msg.orientation.y = sd.quaternion.data[2];
imu_msg.orientation.z = sd.quaternion.data[3];
// Fill angular velocity data
// - scale from deg/s to rad/s
imu_msg.angular_velocity.x = sd.gyroIAlignmentCalibrated.data[0]*3.1415926/180;
imu_msg.angular_velocity.y = sd.gyroIAlignmentCalibrated.data[1]*3.1415926/180;
imu_msg.angular_velocity.z = sd.gyroIAlignmentCalibrated.data[2]*3.1415926/180;
// Fill linear acceleration data
imu_msg.linear_acceleration.x = sd.accCalibrated.data[0]*9.81;
imu_msg.linear_acceleration.y = sd.accCalibrated.data[1]*9.81;
imu_msg.linear_acceleration.z = sd.accCalibrated.data[2]*9.81;
// Publish the messages
imu_pub.publish(imu_msg);
}
}
void run(void)
{
// The timer ensures periodic data publishing
updateTimer = ros::Timer(nh.createTimer(ros::Duration(1.0f/rate),
&LpNAV3Proxy::update,
this));
}
void publishIsAutocalibrationActive()
{
std_msgs::Bool msg;
IG1SettingsI settings;
sensor1->getSettings(settings);
msg.data = settings.enableGyroAutocalibration;
autocalibration_status_pub.publish(msg);
}
///////////////////////////////////////////////////
// Service Callbacks
///////////////////////////////////////////////////
bool setAutocalibration (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_autocalibration");
// clear current settings
IG1SettingsI settings;
sensor1->getSettings(settings);
// Send command
cmdSetEnableAutocalibration(req.data);
ros::Duration(0.2).sleep();
cmdGetEnableAutocalibration();
ros::Duration(0.1).sleep();
double retryElapsedTime = 0;
int retryCount = 0;
while (!sensor1->hasSettings())
{
ros::Duration(0.1).sleep();
ROS_INFO("set_autocalibration wait");
retryElapsedTime += 0.1;
if (retryElapsedTime > 2.0)
{
retryElapsedTime = 0;
cmdGetEnableAutocalibration();
retryCount++;
}
if (retryCount > 5)
break;
}
ROS_INFO("set_autocalibration done");
// Get settings
sensor1->getSettings(settings);
std::string msg;
if (settings.enableGyroAutocalibration == req.data)
{
res.success = true;
msg.append(std::string("[Success] autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
else
{
res.success = false;
msg.append(std::string("[Failed] current autocalibration status set to: ") + (settings.enableGyroAutocalibration?"True":"False"));
}
ROS_INFO("%s", msg.c_str());
res.message = msg;
publishIsAutocalibrationActive();
return res.success;
}
// Auto reconnect
bool setAutoReconnect (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("set_auto_reconnect");
sensor1->setAutoReconnectStatus(req.data);
res.success = true;
std::string msg;
msg.append(std::string("[Success] auto reconnection status set to: ") + (sensor1->getAutoReconnectStatus()?"True":"False"));
ROS_INFO("%s", msg.c_str());
res.message = msg;
return res.success;
}
// reset heading
bool resetHeading (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("reset_heading");
// Send command
cmdResetHeading();
res.success = true;
res.message = "[Success] Heading reset";
return true;
}
bool calibrateGyroscope (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
ROS_INFO("calibrate_gyroscope: Please make sure the sensor is stationary for 4 seconds");
cmdCalibrateGyroscope();
ros::Duration(4).sleep();
res.success = true;
res.message = "[Success] Gyroscope calibration procedure completed";
ROS_INFO("calibrate_gyroscope: Gyroscope calibration procedure completed");
return true;
}
bool getImuData (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
ros::Duration(0.1).sleep();
cmdGetImuData();
res.success = true;
res.message = "[Success] Get imu data";
return true;
}
bool setStreamingMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoStreamingMode();
res.success = true;
res.message = "[Success] Set streaming mode";
return true;
}
bool setCommandMode (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
cmdGotoCommandMode();
res.success = true;
res.message = "[Success] Set command mode";
return true;
}
///////////////////////////////////////////////////
// Helpers
///////////////////////////////////////////////////
void cmdGotoCommandMode ()
{
IG1Command cmd;
cmd.command = GOTO_COMMAND_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGotoStreamingMode ()
{
IG1Command cmd;
cmd.command = GOTO_STREAM_MODE;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetImuData()
{
IG1Command cmd;
cmd.command = GET_IMU_DATA;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdCalibrateGyroscope()
{
IG1Command cmd;
cmd.command = START_GYR_CALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdResetHeading()
{
IG1Command cmd;
cmd.command = SET_ORIENTATION_OFFSET;
cmd.dataLength = 4;
cmd.data.i[0] = LPMS_OFFSET_MODE_HEADING;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdSetEnableAutocalibration(int status)
{
IG1Command cmd;
cmd.command = SET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 4;
cmd.data.i[0] = status;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
void cmdGetEnableAutocalibration()
{
IG1Command cmd;
cmd.command = GET_ENABLE_GYR_AUTOCALIBRATION;
cmd.dataLength = 0;
sensor1->sendCommand(cmd.command, cmd.dataLength, cmd.data.c);
}
private:
// Access to LPMS data
IG1I* sensor1;
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "lpms_be1_node");
ros::NodeHandle nh("imu");
ros::AsyncSpinner spinner(0);
spinner.start();
LpNAV3Proxy lpNAV3(nh);
lpNAV3.run();
ros::waitForShutdown();
return 0;
}
depends/lpmsig1opensourcelib-v0.3.0-20210907-a15087c817ad/ros_example/src/quat_to_euler_node.cpp
+37
View File
@@ -0,0 +1,37 @@
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <iostream>
#include <tf/transform_datatypes.h>
ros::Publisher rpy_publisher;
ros::Subscriber quat_subscriber;
const float r2d = 57.29577951f;
void MsgCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
tf::Quaternion q(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
geometry_msgs::Vector3 rpy;
rpy.x = roll;
rpy.y = pitch;
rpy.z = yaw;
rpy_publisher.publish(rpy);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_listener");
ros::NodeHandle n;
rpy_publisher = n.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);
quat_subscriber = n.subscribe("/imu/data", 1000, MsgCallback);
ROS_INFO("waiting for imu data");
ros::spin();
return 0;
}