在C 中像Roslaunch一样:使用特定类型启动NodeHandle

Question

我是ROS的新手，我正在计划以编程方式启动'video_stream_opencv'节点。

下面的链接：http://wiki.ros.org/video_stream_opencv

启动文件与命令Roslaunch合作良好

<!-- images will be published at /camera_name/image with the image transports plugins (e.g.: compressed) installed -->
<group ns="$(arg camera_name)">
    <node pkg="video_stream_opencv" type="video_stream" name="$(arg camera_name)_stream" output="screen"> 
        <remap from="camera" to="image_raw" />
        <param name="camera_name" type="string" value="$(arg camera_name)" />
        <param name="video_stream_provider" type="string" value="$(arg video_stream_provider)" />
        <param name="fps" type="int" value="$(arg fps)" />
        <param name="frame_id" type="string" value="$(arg frame_id)" />
        <param name="camera_info_url" type="string" value="$(arg camera_info_url)" />
        <param name="flip_horizontal" type="bool" value="$(arg flip_horizontal)" />
        <param name="flip_vertical" type="bool" value="$(arg flip_vertical)" />
        <param name="width" type="int" value="$(arg width)" />
        <param name="height" type="int" value="$(arg height)" />
    </node>
    <node if="$(arg visualize)" name="$(arg camera_name)_image_view" pkg="image_view" type="image_view">
        <remap from="image" to="image_raw" />
    </node>
</group>

，但是现在我试图使用C 代码做同样的事情，但是我看不到如何在节点中设置"类型"属性...

printf("Started ROS threadn");
//glutInit(&argc, argv);
ros::init(argc, argv, "camera_name);
ROS_INFO("camera_name started");

// MY params to set
std::string camera_name = "toto";
std::string video_stream_provider = "toto";
int  fps  = 30;
std::string  frame_id= "toto";
std::string  camera_info_url= "toto";
bool flip_horizontal= false;
bool flip_vertical= false;
int width = 30;
int height = 30;
ros::NodeHandle nh;
ros::M_string remappings;
remappings.insert(std::make_pair("camera", "image_raw"));
ros::NodeHandle node_handle(nh, "camera_name",remappings);
node_handle.setParam("type",video_stream);
node_handle.setParam("camera_name",camera_name);
node_handle.setParam("video_stream_provider",video_stream_provider );
node_handle.setParam("fps",fps);
node_handle.setParam("frame_id",frame_id);
node_handle.setParam("camera_info_url", camera_info_url);
node_handle.setParam("flip_horizontal", flip_horizontal);
node_handle.setParam("flip_vertical",flip_vertical);
node_handle.setParam("width", width);
node_handle.setParam("height", height);

Answer 1

恢复旧线程以帮助有需要的人。

我为此目的创建了一个基于" Rosspawn"的课程。

ros_launch_manager.hpp：

#pragma once
#include <unistd.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/wait.h> 
#include <cstdint>
#include <memory>
#include <thread>
#include <mutex>
#include <vector>
#include <algorithm>
#include <numeric>
#include "ros/ros.h"
#include "ros/console.h"
#include "rospack/rospack.h"
class ROSLaunchManager 
{
    std::vector<pid_t> m_pids;
    std::atomic<bool> m_running;
    std::thread m_thread;
    std::mutex m_mutex;
    
public:
    ROSLaunchManager(ROSLaunchManager const &)
    {
    }
    ROSLaunchManager()
    {
        std::atomic_init(&m_running, true);
        m_thread = std::thread(&ROSLaunchManager::wait, this);
    }
    ~ROSLaunchManager() 
    {
        if (m_running.load()) {
            m_running.store(false);
            if (m_thread.joinable()) {
                m_thread.join();
            }
        }
    }
    template<typename... Args>
    pid_t start(Args... args) 
    {
        std::vector<std::string> args_vector = { args... };
        if (args_vector.size() > 0) {
            pid_t pid = ::fork();
            if (pid == 0) {
                ::setsid();
                
                ::signal(SIGINT, SIG_IGN);
                
                ::fclose(stdout);
                ::fclose(stdin);
                ::fclose(stderr);
                ::execlp("roslaunch", "roslaunch", args..., nullptr);
            }
            else {
                std::scoped_lock<std::mutex> scoped_lock(m_mutex);
                std::string args_string = std::accumulate(std::next(std::begin(args_vector)), std::end(args_vector), args_vector[0], [](std::string lhs, std::string rhs) -> std::string { return lhs + " " + rhs; });
                ROS_INFO("Starting "roslaunch %s" with PID %d", args_string.c_str(), pid);
                m_pids.push_back(pid);
            }
            return pid;
        }
        else {
            throw std::runtime_error("ROSLaunchManager::start - No arguments provided");
        }
    }
    void stop(pid_t const &pid, int32_t const &signal) 
    {
        std::scoped_lock<std::mutex> scoped_lock(m_mutex);
        auto pid_it = std::find(std::begin(m_pids), std::end(m_pids), pid);
        if (pid_it != m_pids.end()) {
            ::kill(pid, signal);
            ROS_INFO("Stopping process with PID %d and signal %d", pid, signal);
        }
        else {
            throw std::runtime_error("ROSLaunchManager::stop - PID " + std::to_string(pid) + " not found");
        }
    }
private:
    void wait()
    {
        while (m_running.load()) {
            std::scoped_lock<std::mutex> scoped_lock(m_mutex);
            for (auto pid_it = std::begin(m_pids); pid_it != std::end(m_pids); ++pid_it) {
                pid_t const pid = *pid_it;
                int32_t status;
                if (::waitpid(pid, &status, WUNTRACED | WCONTINUED | WNOHANG) == pid) {
                    if (WIFEXITED(status)) {
                        ROS_INFO("PID %d exited with status %d", pid, WEXITSTATUS(status));
                        pid_it = m_pids.erase(pid_it);
                        if (pid_it == std::end(m_pids)) {
                            break;
                        }
                    } 
                    else if (WIFSIGNALED(status)) {
                        ROS_INFO("PID %d killed with signal %d", pid, WTERMSIG(status));
                        pid_it = m_pids.erase(pid_it);
                        if (pid_it == std::end(m_pids)) {
                            break;
                        }
                    } 
                    else if (WIFSTOPPED(status)) {
                        ROS_INFO("PID %d stopped with signal %d", pid, WSTOPSIG(status));
                    } 
                    else if (WIFCONTINUED(status)) {
                        ROS_INFO("PID %d continued"   , pid);
                    }
                }
            }
        }
        std::scoped_lock<std::mutex> scoped_lock(m_mutex);
        for (pid_t const &pid : m_pids) {
            ::kill(pid, SIGINT);
            int32_t status;
            ::waitpid(pid, &status, 0);
        }
    }
};

用法：

ROSLaunchManager ros_launch_manager;
pid_t rtabmap_pid;
// Start roslaunch program
try {
    rtabmap_pid = ros_launch_manager.start(
        "rtabmap_ros", "rtabmap.launch", 
        "rtabmap_args:="--delete_db_on_start"", 
        "depth_topic:=/camera/aligned_depth_to_color/image_raw", 
        "rgb_topic:=/camera/color/image_raw",
        "camera_info_topic:=/camera/color/camera_info",
        "approx_sync:=false");
}
catch (std::exception const &exception) {
    ROS_WARN("%s", exception.what());
}
// Stop roslaunch program
try {
    ros_launch_manager.stop(rtabmap_pid, SIGINT);
}
catch (std::exception const &exception) {
    ROS_WARN("%s", exception.what());
}

ros_launch_manager被摧毁时，其所有过程被杀死。使用Sigstop/Sigcont也可以停止/继续这些过程。

Answer 2

我知道启动rosnode的唯一两种方法是使用rosrun或通过启动文件来源。

https://answers.ros.org/question/198279/how-to-to-start-a-ros-node/

Answer 3

我假设您在询问有关rosnode的参数的类型。

在roscpp中，包括/ros/node_handle.h，SetParam（）确实有多个变体，每个变体都设计用于处理特定类型的参数。

 void setParam(const std::string& key, const std::string& s) const;
 void setParam(const std::string& key, const char* s) const;
 void setParam(const std::string& key, double d) const;

等等。

您已经在代码中完成了此操作。