custom plugin using rviz::PoseTool
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?