File name
Commit message
Commit date
File name
Commit message
Commit date
File name
Commit message
Commit date
// Copyright 2022 HarvestX Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "p9n_test/p9n_test_node.hpp"
namespace p9n_test
{
PlayStationTestNode::PlayStationTestNode(const rclcpp::NodeOptions & options)
: rclcpp::Node("p9n_test_node", options)
{
const std::string hw_name = this->declare_parameter<std::string>(
"controller_type", p9n_interface::HW_NAME::DUALSENSE);
try {
this->hw_type_ = p9n_interface::getHwType(hw_name);
} catch (std::runtime_error & e) {
RCLCPP_ERROR(
this->get_logger(),
e.what());
RCLCPP_ERROR(
this->get_logger(),
"Please select hardware from %s",
p9n_interface::getAllHwName().c_str());
rclcpp::shutdown();
return;
}
this->max_trial_ = this->declare_parameter<int>("trial", 3);
if (this->max_trial_ < 1) {
RCLCPP_ERROR(
this->get_logger(),
"Trial should be grater than 1");
rclcpp::shutdown();
return;
}
this->joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
"joy", rclcpp::SensorDataQoS(rclcpp::KeepLast(1)),
std::bind(&PlayStationTestNode::onJoy, this, std::placeholders::_1));
this->wf_handler_ = std::make_unique<WorkflowHandler>();
this->p9n_interface_ =
std::make_unique<p9n_interface::PlayStationInterface>(this->hw_type_);
if (this->joy_sub_->get_publisher_count() == 0) {
RCLCPP_ERROR(
this->get_logger(),
"Joy node not launched");
rclcpp::shutdown();
return;
}
}
void PlayStationTestNode::onJoy(sensor_msgs::msg::Joy::ConstSharedPtr joy_msg)
{
this->p9n_interface_->setJoyMsg(joy_msg);
this->wf_handler_->explainNextTask(true);
if (!this->p9n_interface_->pressedAny()) {
return;
}
bool status = this->wf_handler_->execCurrentTask(this->p9n_interface_);
if (status) {
RCLCPP_INFO(this->get_logger(), "OK");
this->wf_handler_->goNextTask();
} else {
// When Task failed ..
if (!this->wf_handler_->isTrialGreaterThan(this->max_trial_)) {
// You still have chance to try...
RCLCPP_ERROR(
this->get_logger(),
"Invalid input given. Try again...");
} else {
// Game over ...
// Skip current trial.
RCLCPP_ERROR(
this->get_logger(),
"Failed %d times...",
this->max_trial_);
RCLCPP_ERROR(
this->get_logger(),
"Go next");
this->wf_handler_->markCurrentTaskAsFailed();
this->wf_handler_->goNextTask();
return;
}
}
if (this->wf_handler_->isDone()) {
RCLCPP_INFO(this->get_logger(), "Done!");
this->wf_handler_->showResult();
rclcpp::shutdown();
return;
}
// Delay for button chattering
using namespace std::chrono_literals;
rclcpp::sleep_for(500ms);
}
} // namespace p9n_test