Skip to content

Commit 9ddbec3

Browse files
FlovaFlova
committed
Reduce pub overhead
Signed-off-by: Florian Vahl <florian@flova.de>
1 parent 54a9bb5 commit 9ddbec3

1 file changed

Lines changed: 137 additions & 145 deletions

File tree

‎src/lib/livelybot_hardware_sdk/src/yesense_ros/yesense/src/yesense_driver.cpp‎

Lines changed: 137 additions & 145 deletions
Original file line numberDiff line numberDiff line change
@@ -1372,12 +1372,12 @@ void YesenseDriver::spin()
13721372
//maybe this byte is class id
13731373
if(param_class_ == data)
13741374
{
1375-
RCLCPP_INFO(node_->get_logger(), "Almost param response");
1375+
RCLCPP_DEBUG(node_->get_logger(), "Almost param response");
13761376
check_respose_flag_ = true;
13771377
}
13781378
else
13791379
{
1380-
RCLCPP_INFO(node_->get_logger(), "Not param response");
1380+
RCLCPP_DEBUG(node_->get_logger(), "Not param response");
13811381

13821382
check_respose_flag_ = false;
13831383
error_respose_cnt_++;
@@ -1402,7 +1402,8 @@ void YesenseDriver::spin()
14021402

14031403
if(prev_tid != 0 && tid > prev_tid && prev_tid != tid - 1)
14041404
{
1405-
RCLCPP_INFO(node_->get_logger(), "Frame losed: prev_TID: %d, cur_TID: %d", prev_tid, tid);
1405+
RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000,
1406+
"Frame lost: prev_TID: %d, cur_TID: %d", prev_tid, tid);
14061407
}
14071408

14081409
prev_tid = tid;
@@ -1418,12 +1419,12 @@ void YesenseDriver::spin()
14181419
length_low_ = data;
14191420
if(param_id_ == id)
14201421
{
1421-
RCLCPP_INFO(node_->get_logger(), "Double check param response");
1422+
RCLCPP_DEBUG(node_->get_logger(), "Double check param response");
14221423
check_respose_flag_ = true;
14231424
}
14241425
else
14251426
{
1426-
RCLCPP_INFO(node_->get_logger(), "Double not param response");
1427+
RCLCPP_DEBUG(node_->get_logger(), "Double not param response");
14271428
check_respose_flag_ = false;
14281429
}
14291430
}
@@ -1439,7 +1440,7 @@ void YesenseDriver::spin()
14391440
{
14401441
// length is 13-bit
14411442
bytes_ = (length_low_ | data << 8) >> 3;
1442-
RCLCPP_INFO(node_->get_logger(), "package length: %d", bytes_);
1443+
RCLCPP_DEBUG(node_->get_logger(), "package length: %d", bytes_);
14431444
}
14441445
else
14451446
{
@@ -1624,10 +1625,9 @@ void YesenseDriver::update_position_by_gps(const protocol_info_t &imu_data, geom
16241625

16251626
void YesenseDriver::publish_imu(const protocol_info_t &imu_data)
16261627
{
1627-
// publish imu message
1628+
// Always publish the primary IMU topic
16281629
g_imu_.header.stamp = node_->now();
16291630

1630-
// Convert roll/pitch/yaw to quaternion using tf2
16311631
tf2::Quaternion q;
16321632
q.setRPY(
16331633
imu_data.roll / 180.0 * M_PI,
@@ -1649,146 +1649,138 @@ void YesenseDriver::publish_imu(const protocol_info_t &imu_data)
16491649

16501650
imu_pub_->publish(g_imu_);
16511651

1652-
// update imu pose
1653-
geometry_msgs::msg::PoseStamped pose;
1654-
pose.header.frame_id = "imu_link";
1655-
pose.header.stamp = g_imu_.header.stamp;
1656-
pose.pose.position.x = 0.0;
1657-
pose.pose.position.y = 0.0;
1658-
pose.pose.position.z = 0.0;
1659-
pose.pose.orientation.w = g_imu_.orientation.w;
1660-
pose.pose.orientation.x = g_imu_.orientation.x;
1661-
pose.pose.orientation.y = g_imu_.orientation.y;
1662-
pose.pose.orientation.z = g_imu_.orientation.z;
1663-
1664-
// update_position_by_gps(imu_data, pose);
1665-
imu_pose_pub_->publish(pose);
1666-
1667-
// update imu marker
1668-
visualization_msgs::msg::Marker marker_info;
1669-
marker_info.header.frame_id = "imu_link";
1670-
marker_info.header.stamp = node_->now();
1671-
1672-
// set namespace and id
1673-
marker_info.ns = "basic_shapes";
1674-
marker_info.id = 0;
1675-
1676-
// set marker's shape
1677-
marker_info.type = visualization_msgs::msg::Marker::CUBE;
1678-
1679-
// set action: ADD
1680-
marker_info.action = visualization_msgs::msg::Marker::ADD;
1681-
1682-
// set imu pose
1683-
marker_info.pose.position.x = pose.pose.position.x;
1684-
marker_info.pose.position.y = pose.pose.position.y;
1685-
marker_info.pose.position.z = pose.pose.position.z;
1686-
marker_info.pose.orientation.x = g_imu_.orientation.x;
1687-
marker_info.pose.orientation.y = g_imu_.orientation.y;
1688-
marker_info.pose.orientation.z = g_imu_.orientation.z;
1689-
marker_info.pose.orientation.w = g_imu_.orientation.w;
1690-
1691-
// set scale, unit: m
1692-
marker_info.scale.x = 4.0;
1693-
marker_info.scale.y = 4.0;
1694-
marker_info.scale.z = 3.0;
1695-
1696-
// set color
1697-
marker_info.color.r = 0.2f;
1698-
marker_info.color.g = 0.2f;
1699-
marker_info.color.b = 0.2f;
1700-
marker_info.color.a = 1.0;
1701-
1702-
marker_info.lifetime = rclcpp::Duration(0, 0);
1703-
1704-
imu_marker_pub_->publish(marker_info);
1705-
1706-
// publish original sensor data
1707-
yesense_imu::msg::YesenseImuAllData raw_data;
1708-
1709-
raw_data.temperature = imu_data.imu_temp;
1710-
1711-
raw_data.accel.linear.x = imu_data.accel_x;
1712-
raw_data.accel.linear.y = imu_data.accel_y;
1713-
raw_data.accel.linear.z = imu_data.accel_z;
1714-
1715-
raw_data.accel.angular.x = imu_data.angle_x;
1716-
raw_data.accel.angular.y = imu_data.angle_y;
1717-
raw_data.accel.angular.z = imu_data.angle_z;
1718-
1719-
raw_data.euler_angle.roll = imu_data.roll;
1720-
raw_data.euler_angle.pitch = imu_data.pitch;
1721-
raw_data.euler_angle.yaw = imu_data.yaw;
1722-
1723-
raw_data.quaternion.q0 = imu_data.quaternion_data0;
1724-
raw_data.quaternion.q1 = imu_data.quaternion_data1;
1725-
raw_data.quaternion.q2 = imu_data.quaternion_data2;
1726-
raw_data.quaternion.q3 = imu_data.quaternion_data3;
1727-
1728-
raw_data.location.longtidue = imu_data.longtidue;
1729-
raw_data.location.latitude = imu_data.latitude;
1730-
raw_data.location.altidue = imu_data.altidue;
1731-
1732-
raw_data.status.fusion_status = imu_data.status & 0x0f;
1733-
raw_data.status.gnss_status = (imu_data.status >> 4) & 0x0f;
1734-
1735-
// gnss data
1736-
raw_data.gnss.master.utc_time.year = imu_data.gnss.utc_time.year;
1737-
raw_data.gnss.master.utc_time.month = imu_data.gnss.utc_time.month;
1738-
raw_data.gnss.master.utc_time.date = imu_data.gnss.utc_time.date;
1739-
raw_data.gnss.master.utc_time.hour = imu_data.gnss.utc_time.hour;
1740-
raw_data.gnss.master.utc_time.min = imu_data.gnss.utc_time.min;
1741-
raw_data.gnss.master.utc_time.sec = imu_data.gnss.utc_time.sec;
1742-
raw_data.gnss.master.utc_time.ms = imu_data.gnss.utc_time.ms;
1743-
1744-
raw_data.gnss.master.location.longtidue = imu_data.gnss.location.lon;
1745-
raw_data.gnss.master.location.latitude = imu_data.gnss.location.lat;
1746-
raw_data.gnss.master.location.altidue = imu_data.gnss.location.alt;
1747-
1748-
raw_data.gnss.master.location_error.longtidue = imu_data.gnss.location_error.lon;
1749-
raw_data.gnss.master.location_error.latitude = imu_data.gnss.location_error.lat;
1750-
raw_data.gnss.master.location_error.altidue = imu_data.gnss.location_error.alt;
1751-
1752-
raw_data.gnss.master.speed = imu_data.gnss.speed;
1753-
raw_data.gnss.master.yaw = imu_data.gnss.yaw;
1754-
raw_data.gnss.master.status = imu_data.gnss.status;
1755-
raw_data.gnss.master.star_cnt = imu_data.gnss.star_cnt;
1756-
raw_data.gnss.master.p_dop = imu_data.gnss.p_dop;
1757-
raw_data.gnss.master.site_id = imu_data.gnss.site_id;
1758-
1759-
raw_data.gnss.slave.dual_ant_yaw = imu_data.gnss_slave.dual_ant_yaw;
1760-
raw_data.gnss.slave.dual_ant_yaw_error = imu_data.gnss_slave.dual_ant_yaw_error;
1761-
raw_data.gnss.slave.dual_ant_baseline_len = imu_data.gnss_slave.dual_ant_baseline_len;
1762-
1763-
for (auto it = gsp_raw.begin(); it != gsp_raw.end(); ++it)
1652+
// pose — cheap, publish if subscribed
1653+
if (imu_pose_pub_->get_subscription_count() > 0)
17641654
{
1765-
raw_data.gps.raw_data.push_back(it->second);
1655+
geometry_msgs::msg::PoseStamped pose;
1656+
pose.header.frame_id = "imu_link";
1657+
pose.header.stamp = g_imu_.header.stamp;
1658+
pose.pose.orientation = g_imu_.orientation;
1659+
imu_pose_pub_->publish(pose);
1660+
1661+
// path — only if subscribed; capped to avoid ever-growing serialization cost
1662+
if (imu_path_pub_->get_subscription_count() > 0)
1663+
{
1664+
static nav_msgs::msg::Path paths;
1665+
static constexpr size_t kMaxPathPoses = 1000;
1666+
paths.header.frame_id = "imu_link";
1667+
paths.header.stamp = g_imu_.header.stamp;
1668+
paths.poses.push_back(pose);
1669+
if (paths.poses.size() > kMaxPathPoses)
1670+
paths.poses.erase(paths.poses.begin());
1671+
imu_path_pub_->publish(paths);
1672+
}
1673+
}
1674+
1675+
// marker — only if subscribed
1676+
if (imu_marker_pub_->get_subscription_count() > 0)
1677+
{
1678+
visualization_msgs::msg::Marker marker_info;
1679+
marker_info.header.frame_id = "imu_link";
1680+
marker_info.header.stamp = g_imu_.header.stamp;
1681+
marker_info.ns = "basic_shapes";
1682+
marker_info.id = 0;
1683+
marker_info.type = visualization_msgs::msg::Marker::CUBE;
1684+
marker_info.action = visualization_msgs::msg::Marker::ADD;
1685+
marker_info.pose.orientation = g_imu_.orientation;
1686+
marker_info.scale.x = 4.0;
1687+
marker_info.scale.y = 4.0;
1688+
marker_info.scale.z = 3.0;
1689+
marker_info.color.r = 0.2f;
1690+
marker_info.color.g = 0.2f;
1691+
marker_info.color.b = 0.2f;
1692+
marker_info.color.a = 1.0;
1693+
marker_info.lifetime = rclcpp::Duration(0, 0);
1694+
imu_marker_pub_->publish(marker_info);
17661695
}
17671696

1768-
imu_data_pub_->publish(raw_data);
1769-
imu_all_data_pub_->publish(raw_data);
1770-
1771-
// publish sensor data (first fields of AllData)
1772-
yesense_imu::msg::YesenseImuSensorData sensor_data;
1773-
sensor_data.temperature = raw_data.temperature;
1774-
sensor_data.sample_timestamp = raw_data.sample_timestamp;
1775-
sensor_data.sync_timestamp = raw_data.sync_timestamp;
1776-
sensor_data.accel = raw_data.accel;
1777-
sensor_data.quaternion = raw_data.quaternion;
1778-
sensor_data.euler_angle = raw_data.euler_angle;
1779-
sensor_data.location = raw_data.location;
1780-
imu_sensor_data_pub_->publish(sensor_data);
1781-
1782-
imu_gps_data_pub_->publish(raw_data.gps);
1783-
imu_gnss_data_pub_->publish(raw_data.gnss);
1784-
imu_status_pub_->publish(raw_data.status);
1785-
1786-
// publish paths
1787-
static nav_msgs::msg::Path paths;
1788-
paths.header.frame_id = "imu_link";
1789-
paths.header.stamp = node_->now();
1790-
paths.poses.push_back(pose);
1791-
imu_path_pub_->publish(paths);
1697+
// raw/all-data topics — build the message only if someone is listening
1698+
const bool need_raw = imu_data_pub_->get_subscription_count() > 0
1699+
|| imu_all_data_pub_->get_subscription_count() > 0
1700+
|| imu_sensor_data_pub_->get_subscription_count() > 0
1701+
|| imu_gps_data_pub_->get_subscription_count() > 0
1702+
|| imu_gnss_data_pub_->get_subscription_count() > 0
1703+
|| imu_status_pub_->get_subscription_count() > 0;
1704+
1705+
if (need_raw)
1706+
{
1707+
yesense_imu::msg::YesenseImuAllData raw_data;
1708+
1709+
raw_data.temperature = imu_data.imu_temp;
1710+
1711+
raw_data.accel.linear.x = imu_data.accel_x;
1712+
raw_data.accel.linear.y = imu_data.accel_y;
1713+
raw_data.accel.linear.z = imu_data.accel_z;
1714+
1715+
raw_data.accel.angular.x = imu_data.angle_x;
1716+
raw_data.accel.angular.y = imu_data.angle_y;
1717+
raw_data.accel.angular.z = imu_data.angle_z;
1718+
1719+
raw_data.euler_angle.roll = imu_data.roll;
1720+
raw_data.euler_angle.pitch = imu_data.pitch;
1721+
raw_data.euler_angle.yaw = imu_data.yaw;
1722+
1723+
raw_data.quaternion.q0 = imu_data.quaternion_data0;
1724+
raw_data.quaternion.q1 = imu_data.quaternion_data1;
1725+
raw_data.quaternion.q2 = imu_data.quaternion_data2;
1726+
raw_data.quaternion.q3 = imu_data.quaternion_data3;
1727+
1728+
raw_data.location.longtidue = imu_data.longtidue;
1729+
raw_data.location.latitude = imu_data.latitude;
1730+
raw_data.location.altidue = imu_data.altidue;
1731+
1732+
raw_data.status.fusion_status = imu_data.status & 0x0f;
1733+
raw_data.status.gnss_status = (imu_data.status >> 4) & 0x0f;
1734+
1735+
raw_data.gnss.master.utc_time.year = imu_data.gnss.utc_time.year;
1736+
raw_data.gnss.master.utc_time.month = imu_data.gnss.utc_time.month;
1737+
raw_data.gnss.master.utc_time.date = imu_data.gnss.utc_time.date;
1738+
raw_data.gnss.master.utc_time.hour = imu_data.gnss.utc_time.hour;
1739+
raw_data.gnss.master.utc_time.min = imu_data.gnss.utc_time.min;
1740+
raw_data.gnss.master.utc_time.sec = imu_data.gnss.utc_time.sec;
1741+
raw_data.gnss.master.utc_time.ms = imu_data.gnss.utc_time.ms;
1742+
1743+
raw_data.gnss.master.location.longtidue = imu_data.gnss.location.lon;
1744+
raw_data.gnss.master.location.latitude = imu_data.gnss.location.lat;
1745+
raw_data.gnss.master.location.altidue = imu_data.gnss.location.alt;
1746+
1747+
raw_data.gnss.master.location_error.longtidue = imu_data.gnss.location_error.lon;
1748+
raw_data.gnss.master.location_error.latitude = imu_data.gnss.location_error.lat;
1749+
raw_data.gnss.master.location_error.altidue = imu_data.gnss.location_error.alt;
1750+
1751+
raw_data.gnss.master.speed = imu_data.gnss.speed;
1752+
raw_data.gnss.master.yaw = imu_data.gnss.yaw;
1753+
raw_data.gnss.master.status = imu_data.gnss.status;
1754+
raw_data.gnss.master.star_cnt = imu_data.gnss.star_cnt;
1755+
raw_data.gnss.master.p_dop = imu_data.gnss.p_dop;
1756+
raw_data.gnss.master.site_id = imu_data.gnss.site_id;
1757+
1758+
raw_data.gnss.slave.dual_ant_yaw = imu_data.gnss_slave.dual_ant_yaw;
1759+
raw_data.gnss.slave.dual_ant_yaw_error = imu_data.gnss_slave.dual_ant_yaw_error;
1760+
raw_data.gnss.slave.dual_ant_baseline_len = imu_data.gnss_slave.dual_ant_baseline_len;
1761+
1762+
for (auto it = gsp_raw.begin(); it != gsp_raw.end(); ++it)
1763+
raw_data.gps.raw_data.push_back(it->second);
1764+
1765+
if (imu_data_pub_->get_subscription_count() > 0) imu_data_pub_->publish(raw_data);
1766+
if (imu_all_data_pub_->get_subscription_count() > 0) imu_all_data_pub_->publish(raw_data);
1767+
if (imu_status_pub_->get_subscription_count() > 0) imu_status_pub_->publish(raw_data.status);
1768+
if (imu_gps_data_pub_->get_subscription_count() > 0) imu_gps_data_pub_->publish(raw_data.gps);
1769+
if (imu_gnss_data_pub_->get_subscription_count() > 0) imu_gnss_data_pub_->publish(raw_data.gnss);
1770+
1771+
if (imu_sensor_data_pub_->get_subscription_count() > 0)
1772+
{
1773+
yesense_imu::msg::YesenseImuSensorData sensor_data;
1774+
sensor_data.temperature = raw_data.temperature;
1775+
sensor_data.sample_timestamp = raw_data.sample_timestamp;
1776+
sensor_data.sync_timestamp = raw_data.sync_timestamp;
1777+
sensor_data.accel = raw_data.accel;
1778+
sensor_data.quaternion = raw_data.quaternion;
1779+
sensor_data.euler_angle = raw_data.euler_angle;
1780+
sensor_data.location = raw_data.location;
1781+
imu_sensor_data_pub_->publish(sensor_data);
1782+
}
1783+
}
17921784
}
17931785

17941786
}

0 commit comments

Comments
 (0)