ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

custom plugin using rviz::PoseTool

asked 2022-01-05 12:52:29 -0500

mateusguilherme gravatar image

updated 2022-01-05 12:55:48 -0500

hi

I created the following plugin to use with RVIZ, I would like to be able to change the color of the arrow that appears when selecting a position. I would also like to modify the plugin's behavior so that it remains active after a selected position, without having to click on the plugin again.

setpatrolpoint.hpp:

#ifndef SETPATROLPOINT_HPP
#define SETPATROLPOINT_HPP

#ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829
#include <QObject>
#include <ros/ros.h>
#include "rviz/default_plugin/tools/pose_tool.h"
#endif


namespace rviz_plugin_setpatrolpoint_rviz
{

    class SetPatrolPoint : public rviz::PoseTool
    {

      Q_OBJECT
    public:
      SetPatrolPoint();

    protected:
      virtual void onPoseSet(double x, double y, double theta);

    public Q_SLOTS:
      void updateTopic();

    protected:
      ros::NodeHandle nh_;
      ros::Publisher pub_;


    };
}  // namespace rviz_plugin_setpatrolpoint_rviz

#endif  // SETPATROLPOINT_HPP

setpatrolpoint_rviz.cpp

#include "setpatrolpoint_rviz/setpatrolpoint_rviz.hpp"
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include "rviz/display_context.h"

namespace rviz_plugin_setpatrolpoint_rviz
{
    SetPatrolPoint::SetPatrolPoint()
    {

      updateTopic();      
    }

    void SetPatrolPoint::updateTopic()
    {
      pub_ = nh_.advertise<geometry_msgs::PoseStamped>("patrol_point", 1 );
    }

    void SetPatrolPoint::onPoseSet(double x, double y, double theta)
    {

          std::string fixed_frame = context_->getFixedFrame().toStdString();
          geometry_msgs::PoseStamped pose;
          pose.header.frame_id = fixed_frame;
          pose.header.stamp = ros::Time::now();
          pose.pose.position.x = x;
          pose.pose.position.y = y;

          tf::Quaternion quat;
          quat.setRPY(0.0, 0.0, theta);
          tf::quaternionTFToMsg(quat,
                        pose.pose.orientation);
          pub_.publish(pose);

    }

}  // namespace rviz_plugin_setpatrolpoint_rviz

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(rviz_plugin_setpatrolpoint_rviz::SetPatrolPoint, rviz::Tool)

how can I do this?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-01-13 16:35:01 -0500

mateusguilherme gravatar image

I got some ideas in this post: https://answers.ros.org/question/1874...

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2022-01-05 12:52:29 -0500

Seen: 411 times

Last updated: Jan 13 '22