Twistを送るPanel pluginを作る

6080 ワード
ROS2 ROS ROS テキストリンク

【rviz2プラグインの作り方 トップページへ】

マウス操作でTwistを送るプラグインを作ります

実行例

ROS1版

こちらのコードをrviz2用に書き直します
ROS講座74 twistを送るrviz panel pluginを作る

主な変更点

twist_panel.hpp

onInitialize() を追加

public:
    TwistPanel(QWidget *parent = nullptr);

    virtual void onInitialize(); // 追加
    virtual void load(const rviz_common::Config &config);
    virtual void save(rviz_common::Config config) const;

NodeHanlde が存在しないのでNodeに書き換えます
Publisherはtemplateを持たせ得る必要があるので2つ宣言します

protected:
    // The ROS node handle.
    // ros::NodeHandle nh_;
    rclcpp::Node::SharedPtr nh_;

    // The ROS publisher for the command velocity.
    // ros::Publisher twist_publisher_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_publisher_;
    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_stamped_publisher_;

twist_panel.cpp

Node を作成するためにおまじないのようなものを書いています。
今回はコメントアウトしてるコードでもいいんですが、コメントアウトしてるコードを使うとPublisherは動作するんですけどSubscriptionとServiceが動作しなかったのでおまじないを使います。（詳しい方いたら教えていただけるとありがたいです）

void TwistPanel::onInitialize()
{
    nh_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
    // nh_ = std::make_shared<rclcpp::Node>("twist_panel", "sample_rviz_plugin");
}

        if (twist_publisher_ || twist_stamped_publisher_) //
        {
            float vel_max1 = max1_edit_->text().toFloat();
            float vel_max2 = max2_edit_->text().toFloat();
            float vel_max3 = max3_edit_->text().toFloat();

            auto msg = std::make_shared<geometry_msgs::msg::TwistStamped>(); // 
            msg->header.frame_id = pub_frame_;
            // msg->header.stamp = ros::Time::now();
            msg->header.stamp = nh_->now(); //

    // twist_publisher_ = nh_.advertise<geometry_msgs::TwistStamped>(topic_name, 10);
    twist_stamped_publisher_ = nh_->create_publisher<geometry_msgs::msg::TwistStamped>(topic_name, rclcpp::QoS(10));