Skip to content

Commit

Permalink
merged master->ros2 (diagnostics, #1046)
Browse files Browse the repository at this point in the history
  • Loading branch information
matlabbe committed Oct 14, 2023
2 parents 0473206 + 61bd84b commit a3b6f0a
Show file tree
Hide file tree
Showing 34 changed files with 393 additions and 288 deletions.
7 changes: 1 addition & 6 deletions .github/workflows/docker.yml
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ jobs:

strategy:
matrix:
docker_tag: [kinetic, melodic, melodic-latest, noetic, noetic-latest]
docker_tag: [kinetic, melodic, noetic, noetic-latest]
include:
- docker_tag: kinetic
docker_path: 'kinetic'
Expand All @@ -23,11 +23,6 @@ jobs:
linux/amd64
linux/arm64
# linux/arm/v7
- docker_tag: melodic-latest
docker_path: 'melodic/latest'
docker_platforms: |
linux/amd64
linux/arm64
- docker_tag: noetic
docker_path: 'noetic'
docker_platforms: |
Expand Down
2 changes: 1 addition & 1 deletion docker/noetic/superpoint/Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ RUN apt update && \
# Install ros dependencies
RUN apt-get update && \
apt upgrade -y && \
apt-get install -y git ros-noetic-ros-base python3-catkin-tools python3-rosdep build-essential ros-noetic-rtabmap-ros && \
apt-get install -y git ros-noetic-ros-base python3-catkin-tools python3-rosdep build-essential ros-noetic-rtabmap-ros ros-noetic-pybind11-catkin && \
apt-get remove -y ros-noetic-rtabmap && \
rosdep init && \
apt-get clean && rm -rf /var/lib/apt/lists/
Expand Down
95 changes: 95 additions & 0 deletions rtabmap_demos/launch/demo_unitree_quadruped_robot.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
<?xml version="1.0"?>

<launch>

<!-- See https://github.com/unitreerobotics/unitree_guide to bringup simulation.
Fix Cx/Cy of the cameras by setting them to 464 and 400 respectively in unitree_ros/robots/go1_description/xacro/depthCamera.xacro
Ideally, build rtabmap with OpenGV support.
Would work better if simulated environment has a lot of visual texture, see https://github.com/introlab/rtabmap_ros/issues/1031#issuecomment-1722322305
Launch:
$ roslaunch unitree_guide gazeboSim.launch wname:=apt
$ roslaunch rtabmap_demos demo_unitree_quadruped_robot.launch
$ ~/catkin_ws/devel/lib/unitree_guide/junior_ctrl
Press 2 to get up, press 4 to move (w,a,s,d) and rotate (j,l)
-->

<arg name="localization" default="false"/>
<arg if="$(arg localization)" name="rtabmap_args" default="--Mem/IncrementalMemory false"/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--Mem/IncrementalMemory true --delete_db_on_start"/>

<!-- sync rgb/depth images and camera info per camera -->
<group ns="camera_face">
<node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync">
<remap from="rgb/image" to="color/image_raw"/>
<remap from="depth/image" to="depth/image_raw"/>
<remap from="rgb/camera_info" to="color/camera_info"/>
<param name="approx_sync" value="false"/>
</node>
</group>
<group ns="camera_left">
<node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync">
<remap from="rgb/image" to="color/image_raw"/>
<remap from="depth/image" to="depth/image_raw"/>
<remap from="rgb/camera_info" to="color/camera_info"/>
<param name="approx_sync" value="false"/>
</node>
</group>
<group ns="camera_right">
<node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync">
<remap from="rgb/image" to="color/image_raw"/>
<remap from="depth/image" to="depth/image_raw"/>
<remap from="rgb/camera_info" to="color/camera_info"/>
<param name="approx_sync" value="false"/>
</node>
</group>

<group ns="rtabmap">

<!-- sync all cameras together -->
<node pkg="rtabmap_sync" type="rgbdx_sync" name="rgbdx_sync" output="screen">
<remap from="rgbd_image0" to="/camera_left/rgbd_image"/>
<remap from="rgbd_image1" to="/camera_face/rgbd_image"/>
<remap from="rgbd_image2" to="/camera_right/rgbd_image"/>
<param name="rgbd_cameras" type="int" value="3"/>
</node>

<!-- Odometry -->
<node pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="imu" to="/trunk_imu"/>

<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="base"/>
<param name="rgbd_cameras" type="int" value="0"/>
<param name="wait_for_imu_to_init" type="bool" value="true"/>

<param name="Odom/ImageDecimation" type="int" value="2"/>
</node>

<!-- Visual SLAM -->
<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<remap from="imu" to="/trunk_imu"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="rgbd_cameras" type="int" value="0"/>
<param name="frame_id" type="string" value="base"/>

<param name="Grid/RangeMin" type="string" value="0.1"/> <!-- to avoid adding legs as obstacle in occupancy grid map -->
<param name="Grid/MaxObstacleHeight" type="string" value="1"/>
<param name="Mem/ImagePreDecimation" type="string" value="2"/>
<param name="Mem/ImagePostDecimation" type="string" value="2"/>
</node>

<!-- Visualisation RTAB-Map -->
<node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="base"/>
<param name="rgbd_cameras" type="int" value="0"/>
</node>

</group>


</launch>
2 changes: 2 additions & 0 deletions rtabmap_odom/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ find_package(sensor_msgs REQUIRED)
find_package(rtabmap_conversions REQUIRED)
find_package(rtabmap_msgs REQUIRED)
find_package(rtabmap_util REQUIRED)
find_package(rtabmap_sync REQUIRED)

include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/include
Expand All @@ -43,6 +44,7 @@ SET(Libraries
rtabmap_conversions
rtabmap_msgs
rtabmap_util
rtabmap_sync
)

###########
Expand Down
23 changes: 16 additions & 7 deletions rtabmap_odom/include/rtabmap_odom/OdometryROS.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <diagnostic_updater/diagnostic_updater.hpp>

#include <std_srvs/srv/empty.hpp>
#include <std_msgs/msg/header.hpp>
#include <nav_msgs/msg/odometry.hpp>
Expand All @@ -49,14 +51,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <boost/thread.hpp>

#include "rtabmap_util/ULogToRosout.h"
#include "rtabmap_sync/SyncDiagnostic.h"

namespace rtabmap {
class Odometry;
}

namespace rtabmap_odom {

class OdometryROS : public rclcpp::Node
class OdometryROS : public rclcpp::Node, public rtabmap_sync::SyncDiagnostic
{

public:
Expand All @@ -83,9 +86,8 @@ class OdometryROS : public rclcpp::Node

protected:
void init(bool stereoParams, bool visParams, bool icpParams);
void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
void callbackCalled() {callbackCalled_ = true;}
rmw_qos_reliability_policy_t qos() const {return qos_;}
void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");

virtual void flushCallbacks() {};
tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
Expand All @@ -95,7 +97,6 @@ class OdometryROS : public rclcpp::Node
virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}

private:
void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);

virtual void updateParameters(rtabmap::ParametersMap &) {}
virtual void onOdomInit() {}
Expand All @@ -105,9 +106,6 @@ class OdometryROS : public rclcpp::Node

private:
rtabmap::Odometry * odometry_;
std::thread * warningThread_;
std::string subscribedTopicsMsg_;
bool callbackCalled_;

// parameters
std::string frameId_;
Expand Down Expand Up @@ -167,6 +165,17 @@ class OdometryROS : public rclcpp::Node
rtabmap::Transform initialPose_;

rtabmap_util::ULogToRosout ulogToRosout_;

class OdomStatusTask : public diagnostic_updater::DiagnosticTask
{
public:
OdomStatusTask();
void setStatus(bool isLost);
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
private:
bool lost_;
};
OdomStatusTask statusDiagnostic_;
};

}
Expand Down
3 changes: 2 additions & 1 deletion rtabmap_odom/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,8 @@
<depend>rtabmap_conversions</depend>
<depend>rtabmap_msgs</depend>
<depend>rtabmap_util</depend>

<depend>rtabmap_sync</depend>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
73 changes: 45 additions & 28 deletions rtabmap_odom/src/OdometryROS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -62,9 +62,8 @@ OdometryROS::OdometryROS(const rclcpp::NodeOptions & options) :

OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & options) :
Node(name, options),
rtabmap_sync::SyncDiagnostic(this, 0.5),
odometry_(0),
warningThread_(0),
callbackCalled_(false),
frameId_("base_link"),
odomFrameId_("odom"),
groundTruthFrameId_(""),
Expand Down Expand Up @@ -190,13 +189,6 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o

OdometryROS::~OdometryROS()
{
if(warningThread_)
{
callbackCalled();
warningThread_->join();
delete warningThread_;
}

delete odometry_;
}

Expand Down Expand Up @@ -369,28 +361,21 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
onOdomInit();
}

void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
void OdometryROS::initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic)
{
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());

subscribedTopicsMsg_ = subscribedTopicsMsg;
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1.0/5.0);
while(!callbackCalled_)
{
r.sleep();
if(!callbackCalled_)
{
RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
this->get_name(),
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg_.c_str());
}
}
});
std::vector<diagnostic_updater::DiagnosticTask*> tasks;
tasks.push_back(&statusDiagnostic_);
initDiagnostic(subscribedTopic,
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
this->get_name(),
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str()),
tasks);
}

rtabmap::Transform OdometryROS::velocityGuess() const
Expand Down Expand Up @@ -945,6 +930,17 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
{
RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
}

statusDiagnostic_.setStatus(pose.isNull());
if(!pose.isNull())
{
double curentRate = 1.0/(this->now()-timeStart).seconds();
tick(header.stamp,
maxUpdateRate_>0 && maxUpdateRate_ < curentRate ? maxUpdateRate_:
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
}

previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
}
}
Expand Down Expand Up @@ -1046,6 +1042,27 @@ void OdometryROS::setLogError(
ULogger::setLevel(ULogger::kError);
}

OdometryROS::OdomStatusTask::OdomStatusTask() :
diagnostic_updater::DiagnosticTask("Odom status"),
lost_(false)
{}

void OdometryROS::OdomStatusTask::setStatus(bool isLost)
{
lost_ = isLost;
}

void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(lost_)
{
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Lost!");
}
else
{
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Tracking.");
}
}

}

Expand Down
5 changes: 5 additions & 0 deletions rtabmap_odom/src/nodelets/icp_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,11 @@ void ICPOdometry::onOdomInit()
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));

filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));

initDiagnosticMsg(uFormat("\n%s subscribed to %s and %s (make sure only one of this topic is published, otherwise remap one to a dummy topic name).",
get_name(),
scan_sub_->get_topic_name(),
cloud_sub_->get_topic_name()), true);
}

void ICPOdometry::updateParameters(ParametersMap & parameters)
Expand Down
Loading

0 comments on commit a3b6f0a

Please sign in to comment.