ROS_Robot/mapviz
1
0
Fork 0
mirror of https://github.com/swri-robotics/mapviz.git synced 2025-09-15 17:48:34 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

moving follow_waypoints plugin into the follow_waypoints repo

Browse Source
This commit is contained in:
Kevin Nickels 2021-08-19 13:45:07 -05:00
parent 9b066f0d23
commit ce9327b392
7 changed files with 0 additions and 560 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
follow_waypoints/CMakeLists.txt
View File

@ -1,96 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(follow_waypoints_mapviz_plugin)
find_package(catkin REQUIRED COMPONENTS
actionlib
geometry_msgs
mapviz
move_base_msgs
pluginlib
roscpp
std_msgs
)
# Qt Libaries (for mapviz plugin)
find_package(Qt5Core REQUIRED)
find_package(Qt5Gui REQUIRED)
find_package(Qt5OpenGL REQUIRED)
find_package(Qt5Widgets REQUIRED)
# Setting these variable so catkin_package can export Qt as a dependency
set(Qt_FOUND TRUE)
set(Qt_INCLUDE_DIRS "${Qt5Core_INCLUDE_DIRS};${Qt5Gui_INCLUDE_DIRS};${Qt5OpenGL_INCLUDE_DIRS};${Qt5Widgets_INCLUDE_DIRS}")
set(Qt_LIBRARIES
Qt5::Core
Qt5::Gui
Qt5::OpenGL
Qt5::Widgets
)
add_definitions(-DWFlags=WindowFlags)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_plugin ${PROJECT_NAME}_widget
DEPENDS Qt
CATKIN_DEPENDS
actionlib
geometry_msgs
mapviz
move_base_msgs
pluginlib
roscpp
std_msgs
)
# Need to include the current dir to include the results of building Qt UI files
set(CMAKE_INCLUDE_CURRENT_DIR ON)
include_directories(include)
include_directories(SYSTEM
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
# Build libfollow_waypoints_widget
set(UI_FILES
src/follow_waypoints_config.ui
)
set(SRC_FILES
src/follow_waypoints_mapviz_plugin.cpp
)
set(HEADER_FILES
include/follow_waypoints_mapviz_plugin.h
)
qt5_wrap_ui(UI_SRC_FILES ${UI_FILES})
qt5_wrap_cpp(MOC_FILES ${HEADER_FILES})
add_library(${PROJECT_NAME}
${MOC_FILES}
${SRC_FILES}
${HEADER_FILES}
${UI_SRC_FILES}
)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
${Qt_LIBRARIES}
)
set_target_properties(${PROJECT_NAME} PROPERTIES
COMPILE_FLAGS "-std=c++11 -D__STDC_FORMAT_MACROS"
)
### Install the plugins ###
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(TARGETS ${PROJECT_NAME}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
install(FILES mapviz_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

49
follow_waypoints/README.md
View File

@ -1,49 +0,0 @@
# follow_waypoints_mapviz_plugin
A package that will add buttons to control the [follow_waypoints node](http://wiki.ros.org/follow_waypoints)
#### To add a waypoint to the queue:
1. Publish a PoseStamped to the `addpose` topic (`/initialpose` by default). This can be done by the `D Pose Estimate` button in rviz or mapviz (move_base plugin).
2. Publish a PoseStamped to a different `addpose` topic (changing the topic name in the follow_waypoints launchfile or when launching the node)
```
roslaunch follow_waypoints follow_waypoints.launch addpose_topic:=/selected_pose
```
#### To view the current set of waypoints:
```
rostopic echo /waypoints
```
#### To load the previously saved path:
```
rostopic pub /start_journey std_msgs/Empty -1
```
#### To execute waypoint following:
```
rostopic pub /path_ready std_msgs/Empty -1
```
#### To clear the waypoint queue send a "path reset" message:
```
rostopic pub /path_reset std_msgs/Empty -1
```
#### To load the previously save path:
```
rostopic pub /start_journey std_msgs/Empty -1
```
#### OR, you can do all of the above with mapviz plugins:
```
directions go here.
```

5
follow_waypoints/mapviz_plugins.xml
View File

@ -1,5 +0,0 @@
<library path="lib/libfollow_waypoints_mapviz_plugin">
<class name="mapviz_plugins/follow_waypoints" type="mapviz_plugins::FollowWaypointsPlugin" base_class_type="mapviz::MapvizPlugin">
<description>Follow waypoints mapviz plugin.</description>
</class>
</library>

39
follow_waypoints/package.xml
View File

@ -1,39 +0,0 @@
<package format="2">
<name>follow_waypoints_mapviz_plugin</name>
<version>0.3.1</version>
<description>
This mapviz plugin will send a reset, execute, or abort command to the follow_waypoints node.
</description>
<maintainer email="knickels@trinity.edu">Kevin Nickels</maintainer>
<author email="knickels@trinity.edu">Kevin Nickels</author>
<license>BSD</license>
<url type="repository">https://github.com/swri-robotics/mapviz</url>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>qt5-qmake</buildtool_depend>
<build_depend>actionlib</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>rospy</build_depend>
<build_depend>smach</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>qtbase5-dev</build_depend>
<build_depend>libqt5-opengl-dev</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>move_base_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<depend>mapviz</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<exec_depend>libqt5-core</exec_depend>
<exec_depend>libqt5-opengl</exec_depend>
<export>
<mapviz plugin="${prefix}/mapviz_plugins.xml" />
</export>
</package>

BIN
follow_waypoints/readme_images/follow_waypoints_mapviz.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 72 KiB

210
follow_waypoints/src/follow_waypoints_config.ui
View File

@ -1,210 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>follow_waypoints_config</class>
<widget class="QWidget" name="follow_waypoints_config">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>400</width>
<height>96</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<layout class="QFormLayout" name="formLayout">
<item row="2" column="0">
<widget class="QLabel" name="status_label">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="text">
<string>Status:</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="status">
<property name="font">
<font>
<family>Sans Serif</family>
<pointsize>8</pointsize>
</font>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>No topic</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0" colspan="2">
<widget class="QGroupBox" name="groupBox_3">
<property name="minimumSize">
<size>
<width>0</width>
<height>60</height>
</size>
</property>
<property name="title">
<string>follow_waypoints</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
</property>
<widget class="QPushButton" name="pushButtonExecute">
<property name="geometry">
<rect>
<x>110</x>
<y>30</y>
<width>100</width>
<height>24</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>100</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>110</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<pointsize>8</pointsize>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Start executing the current waypoint queue&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="text">
<string>Execute</string>
</property>
<property name="icon">
<iconset>
<normaloff>:/images/green-arrow.png</normaloff>:/images/green-arrow.png</iconset>
</property>
<property name="checkable">
<bool>true</bool>
</property>
</widget>
<widget class="QPushButton" name="pushButtonCancelNav">
<property name="geometry">
<rect>
<x>216</x>
<y>30</y>
<width>100</width>
<height>24</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>100</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<pointsize>8</pointsize>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Cancel the current goal&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="text">
<string>Cancel Nav</string>
</property>
<property name="icon">
<iconset>
<normaloff>:/images/remove-icon-th.png</normaloff>:/images/remove-icon-th.png</iconset>
</property>
<property name="checkable">
<bool>true</bool>
</property>
</widget>
<widget class="QPushButton" name="pushButtonClearQueue">
<property name="geometry">
<rect>
<x>4</x>
<y>30</y>
<width>100</width>
<height>24</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>100</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>65</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<pointsize>8</pointsize>
</font>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Clear the current waypoint queue&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="text">
<string>Clear Queue</string>
</property>
<property name="icon">
<iconset>
<normaloff>:/images/remove-icon-th.png</normaloff>:/images/remove-icon-th.png</iconset>
</property>
<property name="checkable">
<bool>true</bool>
</property>
</widget>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>

161
follow_waypoints/src/follow_waypoints_mapviz_plugin.cpp
View File

@ -1,161 +0,0 @@
// *****************************************************************************
//
// Copyright (c) 2021, Trinity University
// 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 Southwest Research Institute® (SwRI®) 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 <COPYRIGHT HOLDER> 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.
//
// *****************************************************************************
#include <follow_waypoints_mapviz_plugin.h>
// Declare plugin
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mapviz_plugins::FollowWaypointsPlugin, mapviz::MapvizPlugin)
namespace mapviz_plugins
{
FollowWaypointsPlugin::FollowWaypointsPlugin() :
config_widget_(new QWidget())
{
ui_.setupUi(config_widget_);
// Set background white
QPalette p(config_widget_->palette());
p.setColor(QPalette::Background, Qt::white);
config_widget_->setPalette(p);
// Set status text green
ui_.status->setText("OK");
QPalette p3(ui_.status->palette());
p3.setColor(QPalette::Text, Qt::green);
ui_.status->setPalette(p3);
// Execute block
QObject::connect(ui_.pushButtonClearQueue, SIGNAL(toggled(bool)),
this, SLOT(on_pushButtonClearQueue_toggled(bool)));
QObject::connect(ui_.pushButtonExecute, SIGNAL(toggled(bool)),
this, SLOT(on_pushButtonExecute_toggled(bool)));
QObject::connect(ui_.pushButtonCancelNav, SIGNAL(toggled(bool)),
this, SLOT(on_pushButtonCancelNav_toggled(bool)));
}
FollowWaypointsPlugin::~FollowWaypointsPlugin()
{
}
void FollowWaypointsPlugin::PrintError(const std::string& message)
{
PrintErrorHelper( ui_.status, message);
}
void FollowWaypointsPlugin::PrintInfo(const std::string& message)
{
PrintInfoHelper( ui_.status, message);
}
void FollowWaypointsPlugin::PrintWarning(const std::string& message)
{
PrintWarningHelper( ui_.status, message);
}
QWidget* FollowWaypointsPlugin::GetConfigWidget(QWidget* parent)
{
config_widget_->setParent(parent);
return config_widget_;
}
bool FollowWaypointsPlugin::Initialize(QGLWidget* canvas)
{
initialized_ = true;
PrintInfo("FWP Initialized....");
return true;
}
void FollowWaypointsPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
{
PrintInfo("LoadConfig()");
}
void FollowWaypointsPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
{
}
void FollowWaypointsPlugin::on_pushButtonCancelNav_toggled(bool checked)
{
if(checked)
{
PrintInfo("Cancel Navigation...");
actionlib_msgs::GoalID myMsg;
ros::Publisher cancel_pub_ = node_.advertise<actionlib_msgs::GoalID>("/move_base/cancel", 1, true);
sleep(1); // give ROS time to get set up
if (cancel_pub_.getNumSubscribers()==0)
{
PrintWarning("No subscriber to /move_base/cancel topic");
}
cancel_pub_.publish(myMsg);
PrintInfo("published empty msg to /move_base/cancel.... STOP!");
ui_.pushButtonCancelNav->setChecked(false);
}
}
void FollowWaypointsPlugin::on_pushButtonClearQueue_toggled(bool checked)
{
if(checked)
{
PrintInfo("Clear Waypoints...");
std_msgs::Empty myMsg;
ros::Publisher path_pub_ = node_.advertise<std_msgs::Empty>("/path_reset", 1, true);
sleep(1); // give ROS time to get set up
if (path_pub_.getNumSubscribers()==0 )
{
PrintWarning("No subscriber to /path_reset topic");
}
path_pub_.publish(myMsg);
PrintInfo("published empty msg to /path_reset.... CLEAR!");
ui_.pushButtonClearQueue->setChecked(false);
}
}
void FollowWaypointsPlugin::on_pushButtonExecute_toggled(bool checked)
{
if(checked)
{
PrintInfo("Execute Waypoints...");
std_msgs::Empty myMsg;
ros::Publisher path_pub_ = node_.advertise<std_msgs::Empty>("/path_ready", 1, true);
sleep(1); // give ROS time to get set up
if (path_pub_.getNumSubscribers()==0 )
{
PrintWarning("No subscriber to /path_ready topic");
}
path_pub_.publish(myMsg);
PrintInfo("published empty msg to /path_ready.... GO!");
ui_.pushButtonExecute->setChecked(false);
}
}
}