Commit @f19089baa490c0f2b780304ea7a5c2576fc135d4 - yjyoon/ros2_플레이스테이션

m12watanabe1a 2022-05-27

add test

@f19089baa490c0f2b780304ea7a5c2576fc135d4

e65e703

f19089b

p9n_interface/include/p9n_interface/hw_types.hpp

--- p9n_interface/include/p9n_interface/hw_types.hpp

+++ p9n_interface/include/p9n_interface/hw_types.hpp


 #pragma once
 
 #include <string>
+#include <sstream>
 
 namespace p9n_interface
 {

   DUALSENSE,
 };
 
-std::string getHWName(const HW_TYPE & hw_type)
+namespace HW_NAME
 {
-  switch (hw_type) {
-    case HW_TYPE::DUALSHOCK3:
-      return "DualShock3";
-    case HW_TYPE::DUALSHOCK4:
-      return "DualShock4";
-    case HW_TYPE::DUALSENSE:
-      return "DualSense";
-    default:
-      return "Unknown";
-  }
-}
+const char DUALSHOCK3[] = "DualShock3";
+const char DUALSHOCK4[] = "DualShock4";
+const char DUALSENSE[] = "Dualsense";
+}  // namespace HW_NAME
 }  // namespace p9n_interface

e65e703

f19089b

p9n_interface/include/p9n_interface/p9n_interface.hpp

--- p9n_interface/include/p9n_interface/p9n_interface.hpp

+++ p9n_interface/include/p9n_interface/p9n_interface.hpp


   size_t select;
   size_t start;
   size_t PS;
+
+  size_t dpad_up;
+  size_t dpad_down;
+  size_t dpad_right;
+  size_t dpad_left;
 } JoyButtonIdx;
 
 typedef struct

   size_t L2_analog;
 } JoyAxesIdx;
 
+std::string getAllHwName();
+std::string getHwName(const HW_TYPE &);
+HW_TYPE getHwType(const std::string & hw_name);
+
 class PlayStationInterface
 {
 private:
   HW_TYPE HW_TYPE_;
-  sensor_msgs::msg::Joy::UniquePtr joy_;
+  sensor_msgs::msg::Joy::ConstSharedPtr joy_;
   const rclcpp::Logger LOGGER_;
   std::unique_ptr<JoyButtonIdx> btn_idx_;
   std::unique_ptr<JoyAxesIdx> axes_idx_;

 
 public:
   explicit PlayStationInterface(const HW_TYPE);
-  void setJoyMsg(sensor_msgs::msg::Joy::UniquePtr);
+  void setJoyMsg(sensor_msgs::msg::Joy::ConstSharedPtr);
+
+  bool pressedAny();
 
   bool pressedSquare();
   bool pressedCircle();

   bool pressedStart();
   bool pressedPS();
 
+  bool pressedDPadUp();
+  bool pressedDPadDown();
+  bool pressedDPadLeft();
+  bool pressedDPadRight();
+
   float pressedDPadX();
   float pressedDPadY();
 

e65e703

f19089b

p9n_interface/src/p9n_interface.cpp

--- p9n_interface/src/p9n_interface.cpp

+++ p9n_interface/src/p9n_interface.cpp

...	...	@@ -16,6 +16,43 @@
16	16
17	17	namespace p9n_interface
18	18	{
	19	+
	20	+std::string getAllHwName()
	21	+{
	22	+ std::stringstream ss;
	23	+ ss << HW_NAME::DUALSHOCK3 << ", ";
	24	+ ss << HW_NAME::DUALSHOCK4 << ", ";
	25	+ ss << HW_NAME::DUALSENSE;
	26	+ return ss.str();
	27	+}
	28	+
	29	+std::string getHwName(const HW_TYPE & hw_type)
	30	+{
	31	+ switch (hw_type) {
	32	+ case HW_TYPE::DUALSHOCK3:
	33	+ return HW_NAME::DUALSHOCK3;
	34	+ case HW_TYPE::DUALSHOCK4:
	35	+ return HW_NAME::DUALSHOCK4;
	36	+ case HW_TYPE::DUALSENSE:
	37	+ return HW_NAME::DUALSENSE;
	38	+ default:
	39	+ throw std::runtime_error("Invalid hardware type.");
	40	+ }
	41	+}
	42	+
	43	+HW_TYPE getHwType(const std::string & hw_name)
	44	+{
	45	+ if (hw_name == HW_NAME::DUALSHOCK3) {
	46	+ return HW_TYPE::DUALSHOCK3;
	47	+ } else if (hw_name == HW_NAME::DUALSHOCK4) {
	48	+ return HW_TYPE::DUALSHOCK4;
	49	+ } else if (hw_name == HW_NAME::DUALSENSE) {
	50	+ return HW_TYPE::DUALSENSE;
	51	+ }
	52	+ throw std::runtime_error("Invalid hardware name: " + hw_name);
	53	+}
	54	+
	55	+
19	56	PlayStationInterface::PlayStationInterface(
20	57	const HW_TYPE type
21	58	)
...	...	@@ -24,8 +61,8 @@
24	61	{
25	62	RCLCPP_INFO(
26	63	this->LOGGER_,
27		- "Hardware Type: %s",
28		- getHWName(type).c_str());
	64	+ "Hardware: %s",
	65	+ getHwName(this->HW_TYPE_).c_str());
29	66
30	67	this->btn_idx_ = std::make_unique<JoyButtonIdx>();
31	68	this->axes_idx_ = std::make_unique<JoyAxesIdx>();
...	...	@@ -101,9 +138,29 @@
101	138	return false;
102	139	}
103	140
104		-void PlayStationInterface::setJoyMsg(sensor_msgs::msg::Joy::UniquePtr msg)
	141	+void PlayStationInterface::setJoyMsg(sensor_msgs::msg::Joy::ConstSharedPtr msg)
105	142	{
106		- this->joy_ = std::move(msg);
	143	+ this->joy_ = msg;
	144	+}
	145	+
	146	+bool PlayStationInterface::pressedAny()
	147	+{
	148	+ if (!this->isAvailable()) {
	149	+ return false;
	150	+ }
	151	+
	152	+ bool pressed = false;
	153	+ for (auto btn : this->joy_->buttons) {
	154	+ pressed \|= btn;
	155	+ }
	156	+ for (size_t i = 0; i < this->joy_->axes.size(); ++i) {
	157	+ if (i == this->axes_idx_->L2_analog \|\| i == this->axes_idx_->R2_analog) {
	158	+ pressed \|= this->joy_->axes.at(i) < 0.0;
	159	+ continue;
	160	+ }
	161	+ pressed \|= std::abs(this->joy_->axes.at(i)) > 1e-1;
	162	+ }
	163	+ return pressed;
107	164	}
108	165
109	166	bool PlayStationInterface::pressedSquare()
...	...	@@ -205,13 +262,47 @@
205	262	this->btn_idx_->PS);
206	263	}
207	264
	265	+bool PlayStationInterface::pressedDPadUp()
	266	+{
	267	+ return this->pressedDPadY() == 1.0;
	268	+}
	269	+
	270	+bool PlayStationInterface::pressedDPadDown()
	271	+{
	272	+ return this->pressedDPadY() == -1.0;
	273	+}
	274	+
	275	+bool PlayStationInterface::pressedDPadRight()
	276	+{
	277	+ return this->pressedDPadX() == -1.0;
	278	+}
	279	+
	280	+bool PlayStationInterface::pressedDPadLeft()
	281	+{
	282	+ return this->pressedDPadX() == 1.0;
	283	+}
	284	+
208	285	float PlayStationInterface::pressedDPadX()
209	286	{
210	287	if (!this->isAvailable()) {
211	288	return false;
212	289	}
213		- return this->joy_->axes.at(
214		- this->axes_idx_->d_pad_x);
	290	+ switch (this->HW_TYPE_) {
	291	+ case HW_TYPE::DUALSHOCK3:
	292	+ if (this->joy_->buttons.at(this->btn_idx_->dpad_up)) {
	293	+ return 1.0;
	294	+ } else if (this->joy_->buttons.at(this->btn_idx_->dpad_down)) {
	295	+ return -1.0;
	296	+ } else {
	297	+ return 0.0;
	298	+ }
	299	+ case HW_TYPE::DUALSHOCK4:
	300	+ case HW_TYPE::DUALSENSE:
	301	+ return this->joy_->axes.at(
	302	+ this->axes_idx_->d_pad_x);
	303	+ default:
	304	+ throw std::runtime_error("Invalid hardware type");
	305	+ }
215	306	}
216	307
217	308	float PlayStationInterface::pressedDPadY()
...	...	@@ -219,8 +310,22 @@
219	310	if (!this->isAvailable()) {
220	311	return false;
221	312	}
222		- return this->joy_->axes.at(
223		- this->axes_idx_->d_pad_y);
	313	+ switch (this->HW_TYPE_) {
	314	+ case HW_TYPE::DUALSHOCK3:
	315	+ if (this->joy_->buttons.at(this->btn_idx_->dpad_right)) {
	316	+ return -1.0;
	317	+ } else if (this->joy_->buttons.at(this->btn_idx_->dpad_left)) {
	318	+ return 1.0;
	319	+ } else {
	320	+ return 0.0;
	321	+ }
	322	+ case HW_TYPE::DUALSHOCK4:
	323	+ case HW_TYPE::DUALSENSE:
	324	+ return this->joy_->axes.at(
	325	+ this->axes_idx_->d_pad_y);
	326	+ default:
	327	+ throw std::runtime_error("Invalid hardware type");
	328	+ }
224	329	}
225	330
226	331	float PlayStationInterface::tiltedStickLX()

f19089b

p9n_test/.vscode/c_cpp_properties.json (added)

+++ p9n_test/.vscode/c_cpp_properties.json

...	...	@@ -0,0 +1,22 @@
	1	+{
	2	+ "configurations": [
	3	+ {
	4	+ "browse": {
	5	+ "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db",
	6	+ "limitSymbolsToIncludedHeaders": false
	7	+ },
	8	+ "includePath": [
	9	+ "/opt/ros/galactic/include/**",
	10	+ "/usr/include/**",
	11	+ "${workspaceFolder}/include/**",
	12	+ "${workspaceFolder}/../p9n_interface/include/**"
	13	+ ],
	14	+ "name": "ROS",
	15	+ "intelliSenseMode": "clang-x64",
	16	+ "compilerPath": "/usr/bin/gcc",
	17	+ "cStandard": "c99",
	18	+ "cppStandard": "c++17"
	19	+ }
	20	+ ],
	21	+ "version": 4
	22	+}(파일 끝에 줄바꿈 문자 없음)

f19089b

p9n_test/.vscode/cspell.json (added)

+++ p9n_test/.vscode/cspell.json

...	...	@@ -0,0 +1,1 @@
	1	+../../.vscode/cspell.json(파일 끝에 줄바꿈 문자 없음)

f19089b

p9n_test/.vscode/settings.json (added)

+++ p9n_test/.vscode/settings.json

...	...	@@ -0,0 +1,1 @@
	1	+../../.vscode/settings.json(파일 끝에 줄바꿈 문자 없음)

f19089b

p9n_test/.vscode/uncrustify.cfg (added)

+++ p9n_test/.vscode/uncrustify.cfg

...	...	@@ -0,0 +1,1 @@
	1	+../../.vscode/uncrustify.cfg(파일 끝에 줄바꿈 문자 없음)

f19089b

p9n_test/CMakeLists.txt (added)

+++ p9n_test/CMakeLists.txt

...	...	@@ -0,0 +1,32 @@
	1	+cmake_minimum_required(VERSION 3.8)
	2	+project(p9n_test)
	3	+
	4	+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
	5	+ add_compile_options(-Wall -Wextra -Wpedantic)
	6	+endif()
	7	+
	8	+find_package(ament_cmake_auto REQUIRED)
	9	+ament_auto_find_build_dependencies()
	10	+
	11	+
	12	+# Playstation Test ==================================================
	13	+set(TARGET p9n_test_node)
	14	+ament_auto_add_library(
	15	+ ${TARGET}
	16	+ SHARED
	17	+ src/${TARGET}.cpp
	18	+ src/workflow_handler.cpp)
	19	+rclcpp_components_register_node(
	20	+ ${TARGET}
	21	+ PLUGIN "p9n_test::PlayStationTest"
	22	+ EXECUTABLE ${TARGET}_exec)
	23	+# End Playstation Test ==============================================
	24	+
	25	+if(BUILD_TESTING)
	26	+ find_package(ament_lint_auto REQUIRED)
	27	+ set(ament_cmake_copyright_FOUND TRUE)
	28	+ set(ament_cmake_cpplint_FOUND TRUE)
	29	+ ament_lint_auto_find_test_dependencies()
	30	+endif()
	31	+
	32	+ament_auto_package(INSTALL_TO_SHARE launch)

f19089b

p9n_test/include/p9n_test/p9n_test_node.hpp (added)

+++ p9n_test/include/p9n_test/p9n_test_node.hpp

...	...	@@ -0,0 +1,47 @@
	1	+// Copyright 2022 HarvestX Inc.
	2	+//
	3	+// Licensed under the Apache License, Version 2.0 (the "License");
	4	+// you may not use this file except in compliance with the License.
	5	+// You may obtain a copy of the License at
	6	+//
	7	+// http://www.apache.org/licenses/LICENSE-2.0
	8	+//
	9	+// Unless required by applicable law or agreed to in writing, software
	10	+// distributed under the License is distributed on an "AS IS" BASIS,
	11	+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	12	+// See the License for the specific language governing permissions and
	13	+// limitations under the License.
	14	+
	15	+#pragma once
	16	+
	17	+#include <string>
	18	+#include <memory>
	19	+#include <sensor_msgs/msg/joy.hpp>
	20	+#include <rclcpp/rclcpp.hpp>
	21	+
	22	+#include "p9n_test/workflow_handler.hpp"
	23	+#include <p9n_interface/p9n_interface.hpp>
	24	+
	25	+namespace p9n_test
	26	+{
	27	+class PlayStationTest : public rclcpp::Node
	28	+{
	29	+private:
	30	+ p9n_interface::HW_TYPE hw_type_;
	31	+ int max_trial_;
	32	+
	33	+ std::unique_ptr<WorkflowHandler> wf_handler_;
	34	+ std::unique_ptr<
	35	+ p9n_interface::PlayStationInterface> p9n_interface_;
	36	+
	37	+ rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
	38	+
	39	+public:
	40	+ explicit PlayStationTest(const rclcpp::NodeOptions &);
	41	+ void onJoy(sensor_msgs::msg::Joy::ConstSharedPtr);
	42	+
	43	+};
	44	+} // namespace p9n_test
	45	+
	46	+#include "rclcpp_components/register_node_macro.hpp"
	47	+RCLCPP_COMPONENTS_REGISTER_NODE(p9n_test::PlayStationTest)

f19089b

p9n_test/include/p9n_test/workflow_handler.hpp (added)

+++ p9n_test/include/p9n_test/workflow_handler.hpp

...	...	@@ -0,0 +1,78 @@
	1	+// Copyright 2022 HarvestX Inc.
	2	+//
	3	+// Licensed under the Apache License, Version 2.0 (the "License");
	4	+// you may not use this file except in compliance with the License.
	5	+// You may obtain a copy of the License at
	6	+//
	7	+// http://www.apache.org/licenses/LICENSE-2.0
	8	+//
	9	+// Unless required by applicable law or agreed to in writing, software
	10	+// distributed under the License is distributed on an "AS IS" BASIS,
	11	+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	12	+// See the License for the specific language governing permissions and
	13	+// limitations under the License.
	14	+
	15	+#pragma once
	16	+
	17	+#include <string>
	18	+#include <vector>
	19	+#include <memory>
	20	+
	21	+#include <p9n_interface/p9n_interface.hpp>
	22	+
	23	+namespace p9n_test
	24	+{
	25	+
	26	+enum class Task
	27	+{
	28	+ CROSS = 0,
	29	+ CIRCLE,
	30	+ TRIANGLE,
	31	+ SQUARE,
	32	+
	33	+ L1,
	34	+ R1,
	35	+ L2,
	36	+ R2,
	37	+
	38	+ SELECT,
	39	+ START,
	40	+ PS,
	41	+
	42	+ DPAD_UP,
	43	+ DPAD_DOWN,
	44	+ DPAD_RIGHT,
	45	+ DPAD_LEFT,
	46	+
	47	+ STICK_L_LEFT,
	48	+ STICK_L_UP,
	49	+ STICK_R_LEFT,
	50	+ STICK_R_UP,
	51	+
	52	+ END
	53	+};
	54	+
	55	+class WorkflowHandler
	56	+{
	57	+private:
	58	+ int trial_ = 0;
	59	+ int current_task_idx_ = 0;
	60	+ bool done_ = false;
	61	+ rclcpp::Logger LOGGER_ = rclcpp::get_logger("WorkflowHandler");
	62	+
	63	+ std::vector<int> failed_idx_list_;
	64	+
	65	+public:
	66	+ void goNextTask();
	67	+ bool execCurrentTask(
	68	+ std::reference_wrapper<
	69	+ std::unique_ptr<p9n_interface::PlayStationInterface>>);
	70	+ std::string getCurrentTaskStr();
	71	+ std::string getTaskStr(const int);
	72	+ bool isDone();
	73	+ bool isTrialGreaterThan(const int);
	74	+ void explainNextTask(bool = false);
	75	+ void markCurrentTaskAsFailed();
	76	+ void showResult();
	77	+};
	78	+} // namespace p9n_test

f19089b

p9n_test/launch/test.launch.py (added)

+++ p9n_test/launch/test.launch.py

...	...	@@ -0,0 +1,41 @@
	1	+# Copyright 2022 HarvestX Inc.
	2	+#
	3	+# Licensed under the Apache License, Version 2.0 (the "License");
	4	+# you may not use this file except in compliance with the License.
	5	+# You may obtain a copy of the License at
	6	+#
	7	+# http://www.apache.org/licenses/LICENSE-2.0
	8	+#
	9	+# Unless required by applicable law or agreed to in writing, software
	10	+# distributed under the License is distributed on an "AS IS" BASIS,
	11	+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	12	+# See the License for the specific language governing permissions and
	13	+# limitations under the License.
	14	+
	15	+from launch import LaunchDescription
	16	+from launch_ros.actions import ComposableNodeContainer
	17	+from launch_ros.descriptions import ComposableNode
	18	+
	19	+
	20	+def generate_launch_description():
	21	+ """Generate launch description."""
	22	+ nodes = [
	23	+ ComposableNodeContainer(
	24	+ name='joy_container',
	25	+ namespace='p9n',
	26	+ package='rclcpp_components',
	27	+ executable='component_container',
	28	+ composable_node_descriptions=[
	29	+ ComposableNode(
	30	+ package='joy',
	31	+ plugin='joy::Joy',
	32	+ name='joy',
	33	+ ),
	34	+ ComposableNode(
	35	+ package='p9n_test',
	36	+ plugin='p9n_test::PlayStationTest',
	37	+ name='p9n_test'
	38	+ ),
	39	+ ])]
	40	+
	41	+ return LaunchDescription(nodes)

f19089b

p9n_test/package.xml (added)

+++ p9n_test/package.xml

...	...	@@ -0,0 +1,24 @@
	1	+<?xml version="1.0"?>
	2	+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
	3	+<package format="3">
	4	+ <name>p9n_test</name>
	5	+ <version>0.0.0</version>
	6	+ <description>PlayStation JoyController Test</description>
	7	+ <maintainer email="m12watanabe1a@gmail.com">m12watanabe1a</maintainer>
	8	+ <license>Apache License 2.0</license>
	9	+
	10	+ <buildtool_depend>ament_cmake_auto</buildtool_depend>
	11	+
	12	+ <depend>rclcpp</depend>
	13	+ <depend>rclcpp_components</depend>
	14	+ <depend>p9n_interface</depend>
	15	+ <depend>sensor_msgs</depend>
	16	+ <exec_depend>joy</exec_depend>
	17	+
	18	+ <test_depend>ament_lint_auto</test_depend>
	19	+ <test_depend>ament_lint_common</test_depend>
	20	+
	21	+ <export>
	22	+ <build_type>ament_cmake</build_type>
	23	+ </export>
	24	+</package>(파일 끝에 줄바꿈 문자 없음)

f19089b

p9n_test/src/p9n_test_node.cpp (added)

+++ p9n_test/src/p9n_test_node.cpp

...	...	@@ -0,0 +1,117 @@
	1	+// Copyright 2022 HarvestX Inc.
	2	+//
	3	+// Licensed under the Apache License, Version 2.0 (the "License");
	4	+// you may not use this file except in compliance with the License.
	5	+// You may obtain a copy of the License at
	6	+//
	7	+// http://www.apache.org/licenses/LICENSE-2.0
	8	+//
	9	+// Unless required by applicable law or agreed to in writing, software
	10	+// distributed under the License is distributed on an "AS IS" BASIS,
	11	+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	12	+// See the License for the specific language governing permissions and
	13	+// limitations under the License.
	14	+
	15	+#include "p9n_test/p9n_test_node.hpp"
	16	+
	17	+namespace p9n_test
	18	+{
	19	+PlayStationTest::PlayStationTest(const rclcpp::NodeOptions & options)
	20	+: rclcpp::Node("p9n_test_node", options)
	21	+{
	22	+ const std::string hw_name = this->declare_parameter<std::string>(
	23	+ "controller_type", p9n_interface::HW_NAME::DUALSENSE);
	24	+
	25	+ try {
	26	+ this->hw_type_ = p9n_interface::getHwType(hw_name);
	27	+ } catch (std::runtime_error & e) {
	28	+ RCLCPP_ERROR(
	29	+ this->get_logger(),
	30	+ e.what());
	31	+
	32	+ RCLCPP_ERROR(
	33	+ this->get_logger(),
	34	+ "Please select hardware from %s",
	35	+ p9n_interface::getAllHwName().c_str());
	36	+ rclcpp::shutdown();
	37	+ return;
	38	+ }
	39	+
	40	+ this->max_trial_ = this->declare_parameter<int>("trial", 3);
	41	+ if (this->max_trial_ < 1) {
	42	+ RCLCPP_ERROR(
	43	+ this->get_logger(),
	44	+ "Trial should be grater than 1");
	45	+ rclcpp::shutdown();
	46	+ return;
	47	+ }
	48	+
	49	+ this->joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
	50	+ "joy", rclcpp::SensorDataQoS(rclcpp::KeepLast(1)),
	51	+ std::bind(&PlayStationTest::onJoy, this, std::placeholders::_1));
	52	+
	53	+ this->wf_handler_ = std::make_unique<WorkflowHandler>();
	54	+ this->p9n_interface_ =
	55	+ std::make_unique<p9n_interface::PlayStationInterface>(this->hw_type_);
	56	+
	57	+
	58	+ if (this->joy_sub_->get_publisher_count() == 0) {
	59	+ RCLCPP_ERROR(
	60	+ this->get_logger(),
	61	+ "Joy node not launched");
	62	+ rclcpp::shutdown();
	63	+ return;
	64	+ }
	65	+}
	66	+
	67	+void PlayStationTest::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg)
	68	+{
	69	+ this->p9n_interface_->setJoyMsg(joy_msg);
	70	+ this->wf_handler_->explainNextTask(true);
	71	+ if (!this->p9n_interface_->pressedAny()) {
	72	+ return;
	73	+ }
	74	+
	75	+ bool status = this->wf_handler_->execCurrentTask(this->p9n_interface_);
	76	+
	77	+ if (status) {
	78	+ RCLCPP_INFO(this->get_logger(), "OK");
	79	+ this->wf_handler_->goNextTask();
	80	+ } else {
	81	+ // When Task failed ..
	82	+ if (!this->wf_handler_->isTrialGreaterThan(this->max_trial_)) {
	83	+ // You still have chance to try...
	84	+ RCLCPP_ERROR(
	85	+ this->get_logger(),
	86	+ "Invalid input given. Try again...");
	87	+ } else {
	88	+ // Game over ...
	89	+ // Skip current trial.
	90	+ RCLCPP_ERROR(
	91	+ this->get_logger(),
	92	+ "Failed %d times...",
	93	+ this->max_trial_);
	94	+ RCLCPP_ERROR(
	95	+ this->get_logger(),
	96	+ "Go next");
	97	+ this->wf_handler_->markCurrentTaskAsFailed();
	98	+ this->wf_handler_->goNextTask();
	99	+ return;
	100	+ }
	101	+ }
	102	+
	103	+
	104	+ if (this->wf_handler_->isDone()) {
	105	+ RCLCPP_INFO(this->get_logger(), "Done!");
	106	+ this->wf_handler_->showResult();
	107	+ rclcpp::shutdown();
	108	+ return;
	109	+ }
	110	+
	111	+ // Delay for button chattering
	112	+ using namespace std::chrono_literals;
	113	+ rclcpp::sleep_for(500ms);
	114	+}
	115	+
	116	+
	117	+} // namespace p9n_test

f19089b

p9n_test/src/workflow_handler.cpp (added)

+++ p9n_test/src/workflow_handler.cpp

...	...	@@ -0,0 +1,214 @@
	1	+// Copyright 2022 HarvestX Inc.
	2	+//
	3	+// Licensed under the Apache License, Version 2.0 (the "License");
	4	+// you may not use this file except in compliance with the License.
	5	+// You may obtain a copy of the License at
	6	+//
	7	+// http://www.apache.org/licenses/LICENSE-2.0
	8	+//
	9	+// Unless required by applicable law or agreed to in writing, software
	10	+// distributed under the License is distributed on an "AS IS" BASIS,
	11	+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	12	+// See the License for the specific language governing permissions and
	13	+// limitations under the License.
	14	+
	15	+#include "p9n_test/workflow_handler.hpp"
	16	+
	17	+
	18	+namespace p9n_test
	19	+{
	20	+void WorkflowHandler::goNextTask()
	21	+{
	22	+ this->trial_ = 0;
	23	+ this->current_task_idx_ += 1;
	24	+ if (this->current_task_idx_ ==
	25	+ static_cast<size_t>(Task::END))
	26	+ {
	27	+ this->done_ = true;
	28	+ } else {
	29	+ this->explainNextTask();
	30	+ }
	31	+}
	32	+
	33	+bool WorkflowHandler::execCurrentTask(
	34	+ std::reference_wrapper<std::unique_ptr<p9n_interface::PlayStationInterface>>
	35	+ ref_if)
	36	+{
	37	+ this->trial_ += 1;
	38	+ bool trial_result = false;
	39	+
	40	+ switch (this->current_task_idx_) {
	41	+ case static_cast<int>(Task::CROSS):
	42	+ trial_result = ref_if.get()->pressedCross();
	43	+ break;
	44	+ case static_cast<int>(Task::CIRCLE):
	45	+ trial_result = ref_if.get()->pressedCircle();
	46	+ break;
	47	+ case static_cast<int>(Task::TRIANGLE):
	48	+ trial_result = ref_if.get()->pressedTriangle();
	49	+ break;
	50	+ case static_cast<int>(Task::SQUARE):
	51	+ trial_result = ref_if.get()->pressedSquare();
	52	+ break;
	53	+ case static_cast<int>(Task::L1):
	54	+ trial_result = ref_if.get()->pressedL1();
	55	+ break;
	56	+ case static_cast<int>(Task::R1):
	57	+ trial_result = ref_if.get()->pressedR1();
	58	+ break;
	59	+ case static_cast<int>(Task::L2):
	60	+ trial_result = ref_if.get()->pressedL2();
	61	+ break;
	62	+ case static_cast<int>(Task::R2):
	63	+ trial_result = ref_if.get()->pressedR2();
	64	+ break;
	65	+ case static_cast<int>(Task::SELECT):
	66	+ trial_result = ref_if.get()->pressedSelect();
	67	+ break;
	68	+ case static_cast<int>(Task::START):
	69	+ trial_result = ref_if.get()->pressedStart();
	70	+ break;
	71	+ case static_cast<int>(Task::PS):
	72	+ trial_result = ref_if.get()->pressedPS();
	73	+ break;
	74	+ case static_cast<int>(Task::DPAD_UP):
	75	+ trial_result = ref_if.get()->pressedDPadUp();
	76	+ break;
	77	+ case static_cast<int>(Task::DPAD_DOWN):
	78	+ trial_result = ref_if.get()->pressedDPadDown();
	79	+ break;
	80	+ case static_cast<int>(Task::DPAD_RIGHT):
	81	+ trial_result = ref_if.get()->pressedDPadRight();
	82	+ break;
	83	+ case static_cast<int>(Task::DPAD_LEFT):
	84	+ trial_result = ref_if.get()->pressedDPadLeft();
	85	+ break;
	86	+ case static_cast<int>(Task::STICK_L_LEFT):
	87	+ trial_result =
	88	+ ref_if.get()->tiltedStickLX() > 1e-1;
	89	+ break;
	90	+ case static_cast<int>(Task::STICK_L_UP):
	91	+ trial_result =
	92	+ ref_if.get()->tiltedStickLY() > 1e-1;
	93	+ break;
	94	+ case static_cast<int>(Task::STICK_R_LEFT):
	95	+ trial_result =
	96	+ ref_if.get()->tiltedStickRX() > 1e-1;
	97	+ break;
	98	+ case static_cast<int>(Task::STICK_R_UP):
	99	+ trial_result =
	100	+ ref_if.get()->tiltedStickRY() > 1e-1;
	101	+ break;
	102	+ case static_cast<int>(Task::END):
	103	+ default:
	104	+ RCLCPP_ERROR(
	105	+ this->LOGGER_,
	106	+ "Now workflow remains");
	107	+ return true;
	108	+ }
	109	+
	110	+ return trial_result;
	111	+}
	112	+
	113	+std::string WorkflowHandler::getCurrentTaskStr()
	114	+{
	115	+ return this->getTaskStr(this->current_task_idx_);
	116	+}
	117	+
	118	+std::string WorkflowHandler::getTaskStr(const int task_idx)
	119	+{
	120	+ switch (task_idx) {
	121	+ case static_cast<int>(Task::CROSS):
	122	+ return "☓";
	123	+ case static_cast<int>(Task::CIRCLE):
	124	+ return "○";
	125	+ case static_cast<int>(Task::TRIANGLE):
	126	+ return "△";
	127	+ case static_cast<int>(Task::SQUARE):
	128	+ return "□";
	129	+ case static_cast<int>(Task::L1):
	130	+ return "L1";
	131	+ case static_cast<int>(Task::R1):
	132	+ return "R1";
	133	+ case static_cast<int>(Task::L2):
	134	+ return "L2";
	135	+ case static_cast<int>(Task::R2):
	136	+ return "R2";
	137	+ case static_cast<int>(Task::SELECT):
	138	+ return "Select";
	139	+ case static_cast<int>(Task::START):
	140	+ return "Start";
	141	+ case static_cast<int>(Task::PS):
	142	+ return "PS";
	143	+ case static_cast<int>(Task::DPAD_UP):
	144	+ return "Button ↑";
	145	+ case static_cast<int>(Task::DPAD_DOWN):
	146	+ return "Button ↓";
	147	+ case static_cast<int>(Task::DPAD_RIGHT):
	148	+ return "Button →";
	149	+ case static_cast<int>(Task::DPAD_LEFT):
	150	+ return "Button ←";
	151	+ case static_cast<int>(Task::STICK_L_LEFT):
	152	+ return "Stick L ←";
	153	+ case static_cast<int>(Task::STICK_L_UP):
	154	+ return "Stick L ↑";
	155	+ case static_cast<int>(Task::STICK_R_LEFT):
	156	+ return "Stick R ←";
	157	+ case static_cast<int>(Task::STICK_R_UP):
	158	+ return "Stick R ↑";
	159	+ case static_cast<int>(Task::END):
	160	+ default:
	161	+ return "All Task Done";
	162	+ }
	163	+}
	164	+
	165	+bool WorkflowHandler::isDone()
	166	+{
	167	+ return this->done_;
	168	+}
	169	+
	170	+bool WorkflowHandler::isTrialGreaterThan(const int max)
	171	+{
	172	+ return this->trial_ >= max;
	173	+}
	174	+
	175	+void WorkflowHandler::explainNextTask(bool once)
	176	+{
	177	+ const std::string text =
	178	+ "Press [ " + this->getCurrentTaskStr() +
	179	+ " ] button : ";
	180	+ if (once) {
	181	+ RCLCPP_WARN_ONCE(
	182	+ this->LOGGER_,
	183	+ text.c_str());
	184	+ } else {
	185	+ RCLCPP_WARN(
	186	+ this->LOGGER_,
	187	+ text.c_str());
	188	+ }
	189	+}
	190	+
	191	+void WorkflowHandler::markCurrentTaskAsFailed()
	192	+{
	193	+ this->failed_idx_list_.emplace_back(this->current_task_idx_);
	194	+}
	195	+
	196	+void WorkflowHandler::showResult()
	197	+{
	198	+ if (this->failed_idx_list_.empty()) {
	199	+ RCLCPP_INFO(
	200	+ this->LOGGER_,
	201	+ "Passed all task! ��");
	202	+ } else {
	203	+ std::stringstream ss;
	204	+ ss << "Failed target: ";
	205	+ for (auto failed_idx : this->failed_idx_list_) {
	206	+ ss << "[ " <<
	207	+ this->getTaskStr(failed_idx) << " ] ";
	208	+ }
	209	+ RCLCPP_ERROR(
	210	+ this->LOGGER_,
	211	+ ss.str().c_str());
	212	+ }
	213	+}
	214	+} // namespace p9n_test

Add a comment

Open 0
Closed 0

List

...	...	@@ -15,6 +15,7 @@
15	15	#pragma once
16	16
17	17	#include <string>
	18	+#include <sstream>
18	19
19	20	namespace p9n_interface
20	21	{
...	...	@@ -25,17 +26,10 @@
25	26	DUALSENSE,
26	27	};
27	28
28		-std::string getHWName(const HW_TYPE & hw_type)
	29	+namespace HW_NAME
29	30	{
30		- switch (hw_type) {
31		- case HW_TYPE::DUALSHOCK3:
32		- return "DualShock3";
33		- case HW_TYPE::DUALSHOCK4:
34		- return "DualShock4";
35		- case HW_TYPE::DUALSENSE:
36		- return "DualSense";
37		- default:
38		- return "Unknown";
39		- }
40		-}
	31	+const char DUALSHOCK3[] = "DualShock3";
	32	+const char DUALSHOCK4[] = "DualShock4";
	33	+const char DUALSENSE[] = "Dualsense";
	34	+} // namespace HW_NAME
41	35	} // namespace p9n_interface

...	...	@@ -40,6 +40,11 @@
40	40	size_t select;
41	41	size_t start;
42	42	size_t PS;
	43	+
	44	+ size_t dpad_up;
	45	+ size_t dpad_down;
	46	+ size_t dpad_right;
	47	+ size_t dpad_left;
43	48	} JoyButtonIdx;
44	49
45	50	typedef struct
...	...	@@ -56,11 +61,15 @@
56	61	size_t L2_analog;
57	62	} JoyAxesIdx;
58	63
	64	+std::string getAllHwName();
	65	+std::string getHwName(const HW_TYPE &);
	66	+HW_TYPE getHwType(const std::string & hw_name);
	67	+
59	68	class PlayStationInterface
60	69	{
61	70	private:
62	71	HW_TYPE HW_TYPE_;
63		- sensor_msgs::msg::Joy::UniquePtr joy_;
	72	+ sensor_msgs::msg::Joy::ConstSharedPtr joy_;
64	73	const rclcpp::Logger LOGGER_;
65	74	std::unique_ptr<JoyButtonIdx> btn_idx_;
66	75	std::unique_ptr<JoyAxesIdx> axes_idx_;
...	...	@@ -70,7 +79,9 @@
70	79
71	80	public:
72	81	explicit PlayStationInterface(const HW_TYPE);
73		- void setJoyMsg(sensor_msgs::msg::Joy::UniquePtr);
	82	+ void setJoyMsg(sensor_msgs::msg::Joy::ConstSharedPtr);
	83	+
	84	+ bool pressedAny();
74	85
75	86	bool pressedSquare();
76	87	bool pressedCircle();
...	...	@@ -86,6 +97,11 @@
86	97	bool pressedStart();
87	98	bool pressedPS();
88	99
	100	+ bool pressedDPadUp();
	101	+ bool pressedDPadDown();
	102	+ bool pressedDPadLeft();
	103	+ bool pressedDPadRight();
	104	+
89	105	float pressedDPadX();
90	106	float pressedDPadY();
91	107

Delete comment