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

Initial commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/lpms_ig1_ros/CMakeLists.txt
+43
View File
@@ -0,0 +1,43 @@
cmake_minimum_required(VERSION 2.8.3)
project(lpms_ig1_ros)
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
)
## 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})
# 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})
sensors/lpms_ig1_ros/launch/lpmsig1.launch
+21
View File
@@ -0,0 +1,21 @@
<launch>
<!-- IG1 Sensor node -->
<node name="lpms_ig1" pkg="lpms_ig1_ros" type="lpms_ig1_node" output="screen">
<param name="port" value="/dev/ttyUSB1" 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_ros" 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" /> -->
<!-- <node name="plot_acc" pkg="rqt_plot" type="rqt_plot"
args="/acc_vector" /> -->
</launch>
sensors/lpms_ig1_ros/package.xml
+70
View File
@@ -0,0 +1,70 @@
<?xml version="1.0"?>
<package format="2">
<name>lpms_ig1_ros</name>
<version>0.0.0</version>
<description>The lpms_ig1_ros package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="buaazs17@163.com">zhangshu</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 multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/lpms_ig1_ros</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
sensors/lpms_ig1_ros/src/imudata_rad_to_deg_node.cpp
+55
View File
@@ -0,0 +1,55 @@
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <iostream>
#include <tf/transform_datatypes.h>
ros::Publisher angular_vel_deg_publisher;
ros::Publisher acc_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);
geometry_msgs::Vector3 acc_data;
acc_data.x = msg->linear_acceleration.y;
// acc_data.y = msg->linear_acceleration.x;
// acc_data.z = msg->linear_acceleration.z;
acc_publisher.publish(acc_data);
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);
acc_publisher = n.advertise<geometry_msgs::Vector3>("/acc_vector", 1000);
imudata_subscriber = n.subscribe("/imu/data", 1000, MsgCallback);
ROS_INFO("waiting for imu data");
ros::spin();
return 0;
}
sensors/lpms_ig1_ros/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;
}
sensors/lpms_ig1_ros/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;
}