-
Notifications
You must be signed in to change notification settings - Fork 639
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Takamasa Horibe <[email protected]>
- Loading branch information
1 parent
d04f9f1
commit 8c6ddf6
Showing
13 changed files
with
675 additions
and
45 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
2 changes: 2 additions & 0 deletions
2
planning/planning_validator/config/planning_validator.param.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -5,9 +5,11 @@ | |
<version>0.1.0</version> | ||
<description>ros node for planning_validator</description> | ||
<maintainer email="[email protected]">Takamasa Horibe</maintainer> | ||
<maintainer email="[email protected]">Yutaka Shimizu</maintainer> | ||
<license>Apache License 2.0</license> | ||
|
||
<author email="[email protected]">Takamasa Horibe</author> | ||
<author email="[email protected]">Yutaka Shimizu</author> | ||
|
||
<buildtool_depend>ament_cmake_auto</buildtool_depend> | ||
<build_depend>autoware_cmake</build_depend> | ||
|
78 changes: 78 additions & 0 deletions
78
...ning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,78 @@ | ||
// Copyright 2021 Tier IV, 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 "planning_error_monitor/invalid_trajectory_publisher.hpp" | ||
|
||
#include <memory> | ||
#include <string> | ||
#include <utility> | ||
|
||
namespace planning_diagnostics | ||
{ | ||
InvalidTrajectoryPublisherNode::InvalidTrajectoryPublisherNode( | ||
const rclcpp::NodeOptions & node_options) | ||
: Node("invalid_trajectory_publisher", node_options) | ||
{ | ||
using std::placeholders::_1; | ||
using std::chrono_literals::operator""ms; | ||
|
||
traj_sub_ = create_subscription<Trajectory>( | ||
"~/input/trajectory", 1, | ||
std::bind(&InvalidTrajectoryPublisherNode::onCurrentTrajectory, this, _1)); | ||
|
||
traj_pub_ = create_publisher<Trajectory>("~/output/trajectory", 1); | ||
|
||
timer_ = rclcpp::create_timer( | ||
this, get_clock(), 100ms, std::bind(&InvalidTrajectoryPublisherNode::onTimer, this)); | ||
} | ||
|
||
void InvalidTrajectoryPublisherNode::onTimer() | ||
{ | ||
if (!current_trajectory_) { | ||
RCLCPP_INFO(this->get_logger(), "waiting trajectory"); | ||
return; | ||
} | ||
if (current_trajectory_->points.empty()) { | ||
RCLCPP_INFO(this->get_logger(), "waiting trajectory"); | ||
return; | ||
} | ||
|
||
constexpr auto ADDED_VALUE = 1.0e3; | ||
|
||
auto output = *current_trajectory_; | ||
auto & p = output.points.back().pose.position; | ||
p.x += ADDED_VALUE; | ||
p.y += ADDED_VALUE; | ||
p.z += ADDED_VALUE; | ||
|
||
traj_pub_->publish(output); | ||
|
||
RCLCPP_INFO(this->get_logger(), "invalid trajectory is published."); | ||
|
||
bool EXIT_AFTER_PUBLISH = false; | ||
if (EXIT_AFTER_PUBLISH) { | ||
exit(0); | ||
} | ||
} | ||
|
||
void InvalidTrajectoryPublisherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) | ||
{ | ||
current_trajectory_ = msg; | ||
traj_sub_.reset(); | ||
} | ||
|
||
} // namespace planning_diagnostics | ||
|
||
#include <rclcpp_components/register_node_macro.hpp> | ||
RCLCPP_COMPONENTS_REGISTER_NODE(planning_diagnostics::InvalidTrajectoryPublisherNode) |
46 changes: 46 additions & 0 deletions
46
...ning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,46 @@ | ||
// Copyright 2021 Tier IV, 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. | ||
|
||
#ifndef PLANNING_ERROR_MONITOR__INVALID_TRAJECTORY_PUBLISHER_HPP_ | ||
#define PLANNING_ERROR_MONITOR__INVALID_TRAJECTORY_PUBLISHER_HPP_ | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <autoware_auto_planning_msgs/msg/trajectory.hpp> | ||
|
||
#include <string> | ||
|
||
namespace planning_diagnostics | ||
{ | ||
using autoware_auto_planning_msgs::msg::Trajectory; | ||
using autoware_auto_planning_msgs::msg::TrajectoryPoint; | ||
|
||
class InvalidTrajectoryPublisherNode : public rclcpp::Node | ||
{ | ||
public: | ||
explicit InvalidTrajectoryPublisherNode(const rclcpp::NodeOptions & node_options); | ||
void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg); | ||
void onTimer(); | ||
|
||
private: | ||
// ROS | ||
rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_; | ||
rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_; | ||
rclcpp::TimerBase::SharedPtr timer_; | ||
|
||
Trajectory::ConstSharedPtr current_trajectory_ = nullptr; | ||
}; | ||
} // namespace planning_diagnostics | ||
|
||
#endif // PLANNING_ERROR_MONITOR__INVALID_TRAJECTORY_PUBLISHER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
// Copyright 2021 Tier IV, 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 <rclcpp/rclcpp.hpp> | ||
|
||
#include <gtest/gtest.h> | ||
|
||
int main(int argc, char ** argv) | ||
{ | ||
testing::InitGoogleTest(&argc, argv); | ||
rclcpp::init(argc, argv); | ||
bool result = RUN_ALL_TESTS(); | ||
rclcpp::shutdown(); | ||
return result; | ||
} |
Oops, something went wrong.