Skip to content

Commit

Permalink
add src
Browse files Browse the repository at this point in the history
  • Loading branch information
Ar-Ray-code committed Nov 9, 2023
0 parents commit 216184e
Show file tree
Hide file tree
Showing 10 changed files with 667 additions and 0 deletions.
35 changes: 35 additions & 0 deletions .github/workflows/ci_humble.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
name: ci_humble

on:
push:
branches:
- "humble"
pull_request:
types: [opened, synchronize, labeled]

jobs:
ci:
runs-on: ${{ matrix.os }}
if: |
((github.event.action == 'labeled') && (github.event.label.name == 'TESTING') && (github.base_ref == 'humble' )) ||
((github.event.action == 'synchronize') && (github.base_ref == 'humble') && contains(github.event.pull_request.labels.*.name, 'TESTING')) ||
(github.ref_name == 'humble')
container:
image: osrf/ros:${{ matrix.ros_distribution }}-desktop
timeout-minutes: 20
strategy:
fail-fast: false
matrix:
os: [ubuntu-22.04]
ros_distribution: [humble]
steps:
- uses: actions/checkout@v3
- uses: ros-tooling/[email protected]
- name: Build and Test
uses: ros-tooling/[email protected]
with:
target-ros2-distro: ${{ matrix.ros_distribution }}
import-token: ${{ secrets.GITHUB_TOKEN }}
package-name: |
joy_rviz_plugin
vcs-repo-file-url: build_depends.repos
47 changes: 47 additions & 0 deletions joy_rviz_plugin/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
cmake_minimum_required(VERSION 3.8)
project(joy_rviz_plugin)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rviz_common REQUIRED)
find_package(rviz_rendering REQUIRED)

set(CMAKE_AUTOMOC ON)

add_library(${PROJECT_NAME} SHARED
# joy panel
src/joy_panel.cpp
src/touch_widget.cpp
)
ament_target_dependencies(${PROJECT_NAME}
rviz_common
rviz_rendering
)
install(TARGETS
${PROJECT_NAME}
EXPORT ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
)

pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()
7 changes: 7 additions & 0 deletions joy_rviz_plugin/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
## Acknowledgements
This plugin uses ros2_recture from project_srs.

[URL](https://github.com/project-srs/ros2_lecture)

## License
JoyPanel : MIT
21 changes: 21 additions & 0 deletions joy_rviz_plugin/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>joy_rviz_plugin</name>
<version>0.0.0</version>
<description>JoyPanel rviz plugin for CoRE_viewer</description>
<maintainer email="[email protected]">Ar-Ray-code</maintainer>
<license>MIT</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>rviz_common</depend>
<depend>rviz_rendering</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
5 changes: 5 additions & 0 deletions joy_rviz_plugin/plugins_description.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
<library path="joy_rviz_plugin">
<class name="joy_rviz_plugin/JoyPanel" type="joy_rviz_plugin::JoyPanel" base_class_type="rviz_common::Panel">
<description>joy panel</description>
</class>
</library>
94 changes: 94 additions & 0 deletions joy_rviz_plugin/src/joy_handler.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,94 @@
// MIT License

// Copyright (c) 2023 Project SRS

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#pragma once

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joy.hpp>

namespace joy_rviz_plugin
{
class JoyHandler
{
public:
JoyHandler(void) {}

void setRosNodePtr(const rclcpp::Node::SharedPtr node_ptr)
{
node_ptr_ = node_ptr;
}

bool initializePublisher(const std::string topic_name)
{
if (topic_name == "")
{
return false;
}
joy_publisher_ = node_ptr_->create_publisher<sensor_msgs::msg::Joy>(topic_name, rclcpp::QoS(10));
return true;
}

void finalizePublisher(void)
{
joy_publisher_.reset();
}

void publishJoy(std::vector<float> axes, std::vector<int> buttons)
{
sensor_msgs::msg::Joy msg{};
msg.header.stamp = node_ptr_->now();
msg.axes.resize(2);
msg.axes = axes;
msg.buttons = buttons;
joy_publisher_->publish(msg);
}

std::vector<std::string> getTwistTopicList(void) const
{
return getTopicList("sensor_msgs/msg/Joy");
}

private:
std::vector<std::string> getTopicList(const std::string type_name) const
{
std::map<std::string, std::vector<std::string>> topic_map = node_ptr_->get_topic_names_and_types();

std::vector<std::string> output;
for (auto pair : topic_map)
{
for (auto s : pair.second)
{
if (s == type_name)
{
output.push_back(pair.first);
break;
}
}
}
return output;
}

rclcpp::Node::SharedPtr node_ptr_;
rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr joy_publisher_;
};

} // namespace joy_rviz_plugin
179 changes: 179 additions & 0 deletions joy_rviz_plugin/src/joy_panel.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,179 @@
// MIT License

// Copyright (c) 2023 Project SRS

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "joy_panel.hpp"

#include <rviz_common/config.hpp>
#include <rviz_common/display_context.hpp>

#include <QPainter>
#include <QMouseEvent>
#include <QSizePolicy>

namespace joy_rviz_plugin
{
JoyPanel::JoyPanel(QWidget *parent) : rviz_common::Panel(parent)
{
QVBoxLayout *layout = new QVBoxLayout;

QHBoxLayout *layout_1st = new QHBoxLayout;
enable_check_ = new QCheckBox("Topic");
layout_1st->addWidget(enable_check_);
topic_combo_ = new QComboBox();
topic_combo_->setEditable(true);
layout_1st->addWidget(topic_combo_);
layout->addLayout(layout_1st);

touch_ = new TouchWidget();
layout->addWidget(touch_);

QHBoxLayout *layout_3rd = new QHBoxLayout;
a_button_ = new QPushButton("A");
b_button_ = new QPushButton("B");
layout_3rd->addWidget(a_button_);
layout_3rd->addWidget(b_button_);
layout->addLayout(layout_3rd);

setLayout(layout);

interval_timer_ = new QTimer(this);

connect(interval_timer_, &QTimer::timeout, this, &JoyPanel::onTick);
connect(enable_check_, &QCheckBox::stateChanged, this, &JoyPanel::onCheckChange);
connect(a_button_, &QPushButton::clicked, this, &JoyPanel::onClickA);
connect(b_button_, &QPushButton::clicked, this, &JoyPanel::onClickB);
connect(touch_, qOverload<QPointF>(&TouchWidget::notifyScaledPoint), this, &JoyPanel::onTouchChange);

interval_timer_->start(100);
touch_->setEnabled(false);
touch_->update();
}

void JoyPanel::onInitialize()
{
joy_handler_.setRosNodePtr(this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node());
updateTopicList();
}

void JoyPanel::onCheckChange(int state)
{
if (state == Qt::Checked)
{
std::string topic_name = topic_combo_->currentText().toStdString();
bool ret = joy_handler_.initializePublisher(topic_name);
if (!ret)
{
return;
}
topic_combo_->setEnabled(false);
touch_->setEnabled(true);
is_active_ = true;
}
else
{
joy_handler_.finalizePublisher();
topic_combo_->setEnabled(true);
touch_->setEnabled(false);
is_active_ = false;
updateTopicList();
}
}

void JoyPanel::onClickA()
{
a_clicked_ = true;
}

void JoyPanel::onClickB()
{
b_clicked_ = true;
}

void JoyPanel::onTick()
{
if (is_active_)
{
float axis0 = -touch_point_.y();
float axis1 = -touch_point_.x();
joy_handler_.publishJoy({axis0, axis1}, {a_clicked_, b_clicked_});
a_clicked_ = false;
b_clicked_ = false;
}
}

void JoyPanel::save(rviz_common::Config config) const
{
rviz_common::Panel::save(config);
config.mapSetValue("BaseTopic", topic_combo_->currentText());
config.mapSetValue("Checked", enable_check_->isChecked());
}

void JoyPanel::load(const rviz_common::Config &config)
{
rviz_common::Panel::load(config);
QString tmp_text;
bool tmp_bool;
if (config.mapGetString("BaseTopic", &tmp_text))
{
topic_combo_->setCurrentText(tmp_text);
}
if (config.mapGetBool("Checked", &tmp_bool))
{
enable_check_->setChecked(tmp_bool);
}
}

void JoyPanel::updateTopicList(void)
{
std::string previous_topic_name = topic_combo_->currentText().toStdString();
auto topic_list = joy_handler_.getTwistTopicList();
topic_combo_->clear();
int same_topic_index = -1;
for (auto t : topic_list)
{
topic_combo_->addItem(t.c_str());
if (t == previous_topic_name)
{
same_topic_index = topic_combo_->count() - 1;
}
}

if (previous_topic_name != "")
{
if (same_topic_index < 0)
{
topic_combo_->addItem(previous_topic_name.c_str());
same_topic_index = topic_combo_->count() - 1;
}
topic_combo_->setCurrentIndex(same_topic_index);
}
}

void JoyPanel::onTouchChange(QPointF point)
{
touch_point_ = point;
}

} // namespace joy_rviz_plugin

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(joy_rviz_plugin::JoyPanel, rviz_common::Panel)
Loading

0 comments on commit 216184e

Please sign in to comment.