goalpoint_rviz_plugin
我们今天来看far planner里的第二个插件 goalpoint_rviz_plugin。先来看它的头文件:
#ifndef GOALPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H
#define GOALPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H
#include <sstream>
#include <ros/ros.h>
#include <QObject>
#include <sensor_msgs/Joy.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PointStamped.h>
#include "rviz/display_context.h"
#include "rviz/properties/string_property.h"
#include "rviz/default_plugin/tools/pose_tool.h"
namespace rviz
{
class StringProperty;
class GoalPointTool : public PoseTool
{
Q_OBJECT
public:
GoalPointTool();
virtual ~GoalPointTool()
{
}
virtual void onInitialize();
protected:
virtual void odomHandler(const nav_msgs::Odometry::ConstPtr& odom);
virtual void onPoseSet(double x, double y, double theta);
private Q_SLOTS:
void updateTopic();
private:
float vehicle_z;
ros::NodeHandle nh_;
ros::Subscriber sub_;
ros::Publisher pub_;
ros::Publisher pub_joy_;
StringProperty* topic_property_;
};
}
#endif // GOALPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H
和之前的不一样,它是归在rivz下面的。也就是改写了rviz原本的PoseTool,我们可以看到GoalPointTool它继承自PoseTool。主要有这几个函数:
onInitialize()
odomHandler(const nav_msgs::Odometry::ConstPtr& odom)
onPoseSet(double x, double y, double theta)
updateTopic()
我们来看一下他们的实现:
首先是它的构造函数:
GoalPointTool::GoalPointTool()
{
shortcut_key_ = 'w';
topic_property_ = new StringProperty("Topic", "goalpoint", "The topic on which to publish goal waypiont for dynamic route planner.",
getPropertyContainer(), SLOT(updateTopic()), this);
}
rviz: rviz::Tool Class Reference (ros.org)https://docs.ros.org/en/kinetic/api/rviz/html/c++/classrviz_1_1Tool.html shortcut_key应该是快捷键。topic_property @brief Property specialized for string values.
topic_property 中 topic是发送goal waypoint的节点,且默认为goalpoint节点。
/*
* Copyright (c) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef STRING_PROPERTY_H
#define STRING_PROPERTY_H
#include <string>
#include <rviz/properties/property.h>
namespace rviz
{
/** @brief Property specialized for string values. */
class StringProperty : public Property
{
Q_OBJECT
public:
StringProperty(const QString& name = QString(),
const QString& default_value = QString(),
const QString& description = QString(),
Property* parent = nullptr,
const char* changed_slot = nullptr,
QObject* receiver = nullptr);
std::string getStdString()
{
return getValue().toString().toStdString();
}
QString getString()
{
return getValue().toString();
}
public Q_SLOTS:
bool setString(const QString& str)
{
return setValue(str);
}
bool setStdString(const std::string& std_str)
{
return setValue(QString::fromStdString(std_str));
}
};
} // end namespace rviz
#endif // STRING_PROPERTY_H
void GoalPointTool::onInitialize()
{
PoseTool::onInitialize();
setName("Goalpoint");
updateTopic();
vehicle_z = 0;
}
setName("Goalpoint");
Set the name of the tool. This is called by ToolManager during tool initialization. If you want a different name than it gives you, call this from onInitialize() (or thereafter).
void GoalPointTool::updateTopic()
{
sub_ = nh_.subscribe<nav_msgs::Odometry> ("/state_estimation", 5, &GoalPointTool::odomHandler, this);
pub_ = nh_.advertise<geometry_msgs::PointStamped>("/goal_point", 5);
pub_joy_ = nh_.advertise<sensor_msgs::Joy>("/joy", 5);
}
updateTopic主要是订阅和发布节点,其中订阅姿态估计节点,消息类型是nav_msgs::Odometry信息,回调函数是GoalPointTool::odomHandler.
同时,发布节点 /goal_point 消息类型是geometry_msgs::PointStamped。还发布了一个ps2手柄的joy节点 。
joy - ROS Wikihttp://wiki.ros.org/joy
void GoalPointTool::odomHandler(const nav_msgs::Odometry::ConstPtr& odom)
{
vehicle_z = odom->pose.pose.position.z;
}