-
Notifications
You must be signed in to change notification settings - Fork 1
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
オブジェクトの前に移動する関数を追加 #94
base: master
Are you sure you want to change the base?
Changes from 22 commits
ebfe04a
2ca963f
955add6
dd009a0
c7598e3
47b06d7
94e6c36
68a029c
da3b934
28f9be3
a2474b4
8b66efb
8b70792
f4a0e04
c9651fd
61e3c40
c51b76c
d6b7c12
4882696
7954e68
e0cd469
07b98aa
bb0bdb0
75e0b98
da8f665
165f0be
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change | ||||||||||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
@@ -0,0 +1,121 @@ | ||||||||||||||||||||||
// Copyright (c) 2024, OUXT-Polaris | ||||||||||||||||||||||
// | ||||||||||||||||||||||
// 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 <algorithm> | ||||||||||||||||||||||
#include <memory> | ||||||||||||||||||||||
#include <optional> | ||||||||||||||||||||||
#include <string> | ||||||||||||||||||||||
#include <vector> | ||||||||||||||||||||||
|
||||||||||||||||||||||
#include "geometry_msgs/msg/pose_stamped.hpp" | ||||||||||||||||||||||
#include "hermite_path_msgs/msg/planner_status.hpp" | ||||||||||||||||||||||
#include "rclcpp/rclcpp.hpp" | ||||||||||||||||||||||
#include "robotx_behavior_msgs/msg/task_object.hpp" | ||||||||||||||||||||||
#include "robotx_behavior_tree/action_node.hpp" | ||||||||||||||||||||||
|
||||||||||||||||||||||
namespace robotx_behavior_tree | ||||||||||||||||||||||
{ | ||||||||||||||||||||||
class MoveToFrontPoseOfObject : public ActionROS2Node | ||||||||||||||||||||||
{ | ||||||||||||||||||||||
public: | ||||||||||||||||||||||
MoveToFrontPoseOfObject(const std::string & name, const BT::NodeConfiguration & config) | ||||||||||||||||||||||
: ActionROS2Node(name, config) | ||||||||||||||||||||||
{ | ||||||||||||||||||||||
declare_parameter("goal_tolerance", 1.0); | ||||||||||||||||||||||
get_parameter("goal_tolerance", goal_tolerance_); | ||||||||||||||||||||||
goal_pub_front_pose_of_object_ = | ||||||||||||||||||||||
this->create_publisher<geometry_msgs::msg::PoseStamped>("/move_base_simple/goal", 1); | ||||||||||||||||||||||
} | ||||||||||||||||||||||
|
||||||||||||||||||||||
private: | ||||||||||||||||||||||
rclcpp::TimerBase::SharedPtr update_position_timer_; | ||||||||||||||||||||||
float distance_; | ||||||||||||||||||||||
double goal_tolerance_; | ||||||||||||||||||||||
|
||||||||||||||||||||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pub_front_pose_of_object_; | ||||||||||||||||||||||
geometry_msgs::msg::PoseStamped goal_; | ||||||||||||||||||||||
|
||||||||||||||||||||||
std::vector<robotx_behavior_msgs::msg::TaskObject> red_buoys_array_; | ||||||||||||||||||||||
|
||||||||||||||||||||||
enum class Buoy : short { | ||||||||||||||||||||||
BUOY_RED = robotx_behavior_msgs::msg::TaskObject::BUOY_RED, | ||||||||||||||||||||||
BUOY_GREEN = robotx_behavior_msgs::msg::TaskObject::BUOY_GREEN, | ||||||||||||||||||||||
BUOY_WHITE = robotx_behavior_msgs::msg::TaskObject::BUOY_WHITE, | ||||||||||||||||||||||
BUOY_BLACK = robotx_behavior_msgs::msg::TaskObject::BUOY_BLACK | ||||||||||||||||||||||
}; | ||||||||||||||||||||||
enum class Status : short { | ||||||||||||||||||||||
WAITING_FOR_GOAL = hermite_path_msgs::msg::PlannerStatus::WAITING_FOR_GOAL, | ||||||||||||||||||||||
MOVING_TO_GOAL = hermite_path_msgs::msg::PlannerStatus::MOVING_TO_GOAL, | ||||||||||||||||||||||
AVOIDING = hermite_path_msgs::msg::PlannerStatus::MOVING_TO_GOAL | ||||||||||||||||||||||
}; | ||||||||||||||||||||||
|
||||||||||||||||||||||
protected: | ||||||||||||||||||||||
BT::NodeStatus onStart() override | ||||||||||||||||||||||
{ | ||||||||||||||||||||||
const auto status_planner = getPlannerStatus(); | ||||||||||||||||||||||
const auto task_objects_array = getTaskObjects(); | ||||||||||||||||||||||
if (task_objects_array) { | ||||||||||||||||||||||
RCLCPP_INFO(get_logger(), "get task_objects"); | ||||||||||||||||||||||
return BT::NodeStatus::RUNNING; | ||||||||||||||||||||||
} else { | ||||||||||||||||||||||
return BT::NodeStatus::FAILURE; | ||||||||||||||||||||||
} | ||||||||||||||||||||||
} | ||||||||||||||||||||||
|
||||||||||||||||||||||
BT::NodeStatus onRunning() override | ||||||||||||||||||||||
{ | ||||||||||||||||||||||
const auto status_planner = getPlannerStatus(); | ||||||||||||||||||||||
const auto pose = getCurrentPose(); | ||||||||||||||||||||||
const auto task_objects_array = getTaskObjects(); | ||||||||||||||||||||||
|
||||||||||||||||||||||
if (task_objects_array) { | ||||||||||||||||||||||
red_buoys_array_ = filter(task_objects_array.value(), static_cast<short>(Buoy::BUOY_RED)); | ||||||||||||||||||||||
} | ||||||||||||||||||||||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. 何色のブイの前に行きたいかは状況次第なので、behavior treeの構造を決めるxmlから指示できるようにしておいてください。 There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. イメージとしてはこんな感じ
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. robotx_behavior_tree/robotx_behavior_tree/plugins/action/move_goal_action.cpp Lines 45 to 51 in 739c925
robotx_behavior_tree/robotx_behavior_tree/plugins/action/move_goal_action.cpp Lines 56 to 58 in 739c925
こんな感じで書くと、xmlから値を読み込めます。 |
||||||||||||||||||||||
|
||||||||||||||||||||||
sortBy2DDistance(red_buoys_array_, pose.value()->pose.position); | ||||||||||||||||||||||
if (red_buoys_array_.empty()) { | ||||||||||||||||||||||
return BT::NodeStatus::FAILURE; | ||||||||||||||||||||||
} | ||||||||||||||||||||||
|
||||||||||||||||||||||
const auto xyz = getFrontPoseOfObject(red_buoys_array_[0], 5.0); | ||||||||||||||||||||||
get_parameter("goal_tolerance", goal_tolerance_); | ||||||||||||||||||||||
goal_.header.frame_id = "map"; | ||||||||||||||||||||||
if (xyz) { | ||||||||||||||||||||||
goal_.pose.position.x = xyz.value().position.x; | ||||||||||||||||||||||
goal_.pose.position.y = xyz.value().position.y; | ||||||||||||||||||||||
goal_.pose.position.z = xyz.value().position.z; | ||||||||||||||||||||||
goal_.pose.orientation.w = xyz.value().orientation.w; | ||||||||||||||||||||||
goal_.pose.orientation.x = xyz.value().orientation.x; | ||||||||||||||||||||||
goal_.pose.orientation.y = xyz.value().orientation.y; | ||||||||||||||||||||||
goal_.pose.orientation.z = xyz.value().orientation.z; | ||||||||||||||||||||||
} | ||||||||||||||||||||||
goal_.header.stamp = get_clock()->now(); | ||||||||||||||||||||||
if (status_planner.value()->status == static_cast<short>(Status::WAITING_FOR_GOAL)) { | ||||||||||||||||||||||
goal_pub_front_pose_of_object_->publish(goal_); | ||||||||||||||||||||||
} | ||||||||||||||||||||||
distance_ = getDistance(pose.value()->pose.position, goal_.pose.position); | ||||||||||||||||||||||
|
||||||||||||||||||||||
if (distance_ < goal_tolerance_) { | ||||||||||||||||||||||
RCLCPP_INFO(get_logger(), "Throgh Goal : SUCCESS"); | ||||||||||||||||||||||
return BT::NodeStatus::SUCCESS; | ||||||||||||||||||||||
} | ||||||||||||||||||||||
|
||||||||||||||||||||||
return BT::NodeStatus::RUNNING; | ||||||||||||||||||||||
} | ||||||||||||||||||||||
}; | ||||||||||||||||||||||
} // namespace robotx_behavior_tree | ||||||||||||||||||||||
|
||||||||||||||||||||||
#include "behavior_tree_action_builder/register_nodes.hpp" // NOLINT | ||||||||||||||||||||||
|
||||||||||||||||||||||
REGISTER_NODES(robotx_behavior_tree, MoveToFrontPoseOfObject) |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,56 @@ | ||
// Copyright (c) 2023 OUXT Polaris | ||
// | ||
// 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. | ||
|
||
/** | ||
* @file test_action_node.cpp | ||
* @author Kento Hirogaki [email protected] | ||
* @brief test code for Action Node | ||
* @version 0.1 | ||
* @date 2023-10-17 | ||
* | ||
* @copyright Copyright (c) 2023 | ||
* | ||
*/ | ||
#include <gtest/gtest.h> | ||
|
||
#include <robotx_behavior_tree/action_node.hpp> | ||
|
||
TEST(TestSuite, testCase1) | ||
{ | ||
robotx_behavior_msgs::msg::TaskObject object; | ||
// robotx_behavior_tree::ActionROS2Node *testtest(); | ||
object.x = 0; | ||
object.y = 0; | ||
|
||
// object->x = 0; | ||
// object->y = 0; | ||
// robotx_behavior_tree::ActionROS2Node::getFrontPoseForWaypoint(object, 2.0); | ||
|
||
// robotx_behavior_tree::getFrontPoseForWaypoint(object, 2.0); | ||
// robotx_behavior_tree::ActionROS2Node::getPoint(object); | ||
EXPECT_EQ(true, true); | ||
} | ||
|
||
/** | ||
* @brief Run all the tests that were declared with TEST() | ||
* | ||
* @param argc | ||
* @param argv | ||
* @return int | ||
*/ | ||
int main(int argc, char ** argv) | ||
{ | ||
testing::InitGoogleTest(&argc, argv); | ||
return RUN_ALL_TESTS(); | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,7 @@ | ||
<root main_tree_to_execute = "MainTree" > | ||
<BehaviorTree ID="MainTree"> | ||
<Sequence> | ||
<Action ID="MoveToFrontPoseOfObject" task_objects="{task_objects}"/> | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. portの追加をお願いします! |
||
</Sequence> | ||
</BehaviorTree> | ||
</root> |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,9 @@ | ||
plugins: | ||
- package: robotx_behavior_tree | ||
name: | ||
- move_to_front_pose_of_object | ||
- move_goal_action | ||
behavior: | ||
description: | ||
package: robotx_bt_planner | ||
path : behavior_trees/move_to_front_pose_of_object.xml |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
指定したオブジェクトの前に移動してほしいというActionなのでこの変数は不要かと思います。
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
ゴール地点のtask_objectがあれば良いので、
みたいな型で情報を保存しておくのが望ましいと思います。