forked from CoreSenseEU/CoreSense4Home
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
74 changed files
with
2,361 additions
and
570 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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,37 @@ | ||
| name: main | ||
|
|
||
| on: | ||
| pull_request: | ||
| branches: | ||
| - main | ||
| push: | ||
| branches: | ||
| - main | ||
| schedule: | ||
| - cron: '0 0 * * 6' | ||
| jobs: | ||
| build-and-test: | ||
| runs-on: ${{ matrix.os }} | ||
| strategy: | ||
| matrix: | ||
| os: [ubuntu-22.04] | ||
| fail-fast: false | ||
| steps: | ||
| - name: Setup ROS 2 | ||
| uses: ros-tooling/[email protected] | ||
| with: | ||
| required-ros-distributions: humble | ||
| - name: Install nlohmann | ||
| run: sudo apt -y install nlohmann-json3-dev | ||
| - name: build and test | ||
| uses: ros-tooling/[email protected] | ||
| with: | ||
| package-name: arm bt_test hri motion perception robocup_bringup cs4home_core | ||
| target-ros2-distro: humble | ||
| colcon-defaults: | | ||
| { | ||
| "test": { | ||
| "parallel-workers" : 1 | ||
| } | ||
| } | ||
| vcs-repo-file-url: https://raw.githubusercontent.com/CoreSenseEU/CoreSense4Home/main/robocup_bringup/thirdparty.repos |
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
59 changes: 59 additions & 0 deletions
59
bt_nodes/arm/include/arm/manipulation/move_to_predefined.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,59 @@ | ||
| // Copyright 2024 Intelligent Robotics Lab - Gentlebots | ||
| // | ||
| // 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 andGO2OBJECT | ||
| // limitations under the License. | ||
|
|
||
| #ifndef ARM_MANIPULATION__MOVE_TO_PREDEFINED_HPP_ | ||
| #define ARM_MANIPULATION__MOVE_TO_PREDEFINED_HPP_ | ||
|
|
||
| #include <algorithm> | ||
| #include <string> | ||
|
|
||
| #include "manipulation_interfaces/action/move_to_predefined.hpp" | ||
| #include "moveit_msgs/msg/collision_object.hpp" | ||
| #include "behaviortree_cpp_v3/behavior_tree.h" | ||
| #include "behaviortree_cpp_v3/bt_factory.h" | ||
| #include "arm/manipulation/BTActionNode.hpp" | ||
| #include "rclcpp/rclcpp.hpp" | ||
|
|
||
| namespace manipulation | ||
| { | ||
|
|
||
| class MoveToPredefined : public manipulation::BtActionNode<manipulation_interfaces::action::MoveToPredefined> | ||
| { | ||
| public: | ||
| explicit MoveToPredefined( | ||
| const std::string & xml_tag_name, | ||
| const std::string & action_name, | ||
| const BT::NodeConfiguration & conf); | ||
|
|
||
| void on_tick() override; | ||
| BT::NodeStatus on_success() override; | ||
|
|
||
| static BT::PortsList providedPorts() | ||
| { | ||
| return BT::PortsList( | ||
| {BT::InputPort<std::string>( | ||
| "pose")}); | ||
| } | ||
|
|
||
| private: | ||
| // rclcpp::Node::SharedPtr node_; | ||
| // rclcpp::ActionClient<audio_common_msgs::action::TTS>::SharedPtr | ||
| // tts_action_; | ||
|
|
||
| std::string pose_; | ||
| }; | ||
|
|
||
| } // namespace manipulation | ||
|
|
||
| #endif // ARM_MANIPULATION__MOVE_TO_PREDEFINED_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,55 @@ | ||
| // Copyright 2024 Intelligent Robotics Lab - Gentlebots | ||
| // | ||
| // 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 <string> | ||
| #include <utility> | ||
|
|
||
| #include "arm/manipulation/move_to_predefined.hpp" | ||
| #include "manipulation_interfaces/action/move_to_predefined.hpp" | ||
| #include "behaviortree_cpp_v3/behavior_tree.h" | ||
|
|
||
| namespace manipulation | ||
| { | ||
|
|
||
| using namespace std::chrono_literals; | ||
| using namespace std::placeholders; | ||
|
|
||
| MoveToPredefined::MoveToPredefined( | ||
| const std::string & xml_tag_name, const std::string & action_name, | ||
| const BT::NodeConfiguration & conf) | ||
| : manipulation::BtActionNode<manipulation_interfaces::action::MoveToPredefined>(xml_tag_name, | ||
| action_name, conf) {} | ||
|
|
||
| void MoveToPredefined::on_tick() | ||
| { | ||
|
|
||
| RCLCPP_DEBUG(node_->get_logger(), "MoveToPredefined ticked"); | ||
|
|
||
| getInput("pose", pose_); | ||
| goal_.group_name = "arm_torso"; | ||
| goal_.goal_pose = pose_; | ||
| } | ||
|
|
||
| BT::NodeStatus MoveToPredefined::on_success() {return BT::NodeStatus::SUCCESS;} | ||
|
|
||
| } // namespace manipulation | ||
| #include "behaviortree_cpp_v3/bt_factory.h" | ||
| BT_REGISTER_NODES(factory) { | ||
| BT::NodeBuilder builder = [](const std::string & name, | ||
| const BT::NodeConfiguration & config) { | ||
| return std::make_unique<manipulation::MoveToPredefined>(name, "/move_to_predefined", config); | ||
| }; | ||
|
|
||
| factory.registerBuilder<manipulation::MoveToPredefined>("MoveToPredefined", builder); | ||
| } |
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 |
|---|---|---|
| @@ -0,0 +1,102 @@ | ||
| <?xml version="1.0"?> | ||
| <root main_tree_to_execute="BehaviorTree"> | ||
| <!-- ////////// --> | ||
| <BehaviorTree ID="BehaviorTree"> | ||
| <Sequence> | ||
| <Action ID="InitCarry"/> | ||
| <RetryUntilSuccessful num_attempts="-1"> | ||
| <Sequence> | ||
| <Condition ID="IsDetected" cam_frame="head_front_camera_link_color_optical_frame" confidence="0.6" frames="{frames}" interest="person" max_depth="3" max_entities="1" order="color"/> | ||
| <Condition ID="IsPointing" bag_frame="{bag_tf}" cam_frame="head_front_camera_link_color_optical_frame"/> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="-1"> | ||
| <Action ID="MoveTo" distance_tolerance="1.0" tf_frame="{bag_tf}" will_finish="true"/> | ||
| </RetryUntilSuccessful> | ||
| <Action ID="MoveToPredefined" pose="offer"/> | ||
| <ReactiveSequence> | ||
| <Action ID="LookAt" tf_frame="person_0" tf_frames=""/> | ||
| <Action ID="Speak" param="" say_text="please put the bag in my gripper"/> | ||
| </ReactiveSequence> | ||
| <Repeat num_cycles="5"> | ||
| <Action ID="LookAt" tf_frame="gripper_link" tf_frames=""/> | ||
| </Repeat> | ||
| <Delay delay_msec="5000"> | ||
| <Action ID="MoveToPredefined" pose="home"/> | ||
| </Delay> | ||
| <ReactiveSequence> | ||
| <Action ID="LookAt" tf_frame="person_0" tf_frames=""/> | ||
| <Action ID="Speak" param="" say_text="Perfect, now i will follow you. Please stop at the end "/> | ||
| </ReactiveSequence> | ||
| <RetryUntilSuccessful num_attempts="-1"> | ||
| <Fallback> | ||
| <ReactiveSequence> | ||
| <RetryUntilSuccessful num_attempts="-1"> | ||
| <Condition ID="IsDetected" cam_frame="head_front_camera_link_color_optical_frame" confidence="0.3" frames="" interest="person" max_depth="6" max_entities="1" order="color"/> | ||
| </RetryUntilSuccessful> | ||
| <Action ID="LookAt" tf_frame="person_0"/> | ||
| <Condition ID="IsEntityMoving" velocity_tolerance="0.12" frame="person_0" max_iterations="100"/> | ||
| <Action ID="MoveTo" distance_tolerance="0.2" tf_frame="person_0" will_finish="false"/> | ||
| </ReactiveSequence> | ||
| <ForceFailure> | ||
| <Action ID="Speak" param="" say_text="have we arrived to the destination?"/> | ||
| </ForceFailure> | ||
| <Action ID="DialogConfirmation"/> | ||
| </Fallback> | ||
| </RetryUntilSuccessful> | ||
| <Action ID="MoveToPredefined" pose="offer"/> | ||
| <ReactiveSequence> | ||
| <Action ID="LookAt" tf_frame="person_0" tf_frames=""/> | ||
| <Action ID="Speak" param="" say_text="Here is the bag, please take it"/> | ||
| </ReactiveSequence> | ||
| <Action ID="LookAt" tf_frame="gripper_link"/> | ||
| <Action ID="MoveToPredefined" pose="home"/> | ||
| <Delay delay_msec="3000"> | ||
| <Action ID="Speak" param="" say_text="I am going back, have a nice day"/> | ||
| </Delay> | ||
| <Action ID="MoveTo" distance_tolerance="0.0" tf_frame="odom"/> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
| <!-- ////////// --> | ||
| <TreeNodesModel> | ||
| <Action ID="DialogConfirmation"/> | ||
| <Action ID="InitCarry"/> | ||
| <Condition ID="IsDetected"> | ||
| <input_port default="head_front_camera_link_color_optical_frame" name="cam_frame"/> | ||
| <input_port default="0.6" name="confidence"/> | ||
| <output_port name="frames">array of frames</output_port> | ||
| <input_port default="person" name="interest"/> | ||
| <input_port default="2" name="max_depth">value in meters</input_port> | ||
| <input_port default="1" name="max_entities"/> | ||
| <input_port default="depth_nearest" name="order">[depth, color]</input_port> | ||
| </Condition> | ||
| <Condition ID="IsEntityMoving"> | ||
| <input_port name="check_time"/> | ||
| <input_port name="distance_tolerance"/> | ||
| <input_port name="frame"/> | ||
| </Condition> | ||
| <Condition ID="IsPointing"> | ||
| <output_port name="bag_frame"/> | ||
| <input_port name="cam_frame"/> | ||
| </Condition> | ||
| <Action ID="Listen"> | ||
| <output_port name="listen_text"/> | ||
| </Action> | ||
| <Action ID="LookAt"> | ||
| <input_port name="tf_frame"/> | ||
| <input_port name="tf_frames"/> | ||
| </Action> | ||
| <Action ID="MoveTo"> | ||
| <input_port default="1.0" name="distance_tolerance"/> | ||
| <input_port name="tf_frame"/> | ||
| </Action> | ||
| <Action ID="MoveToPredefined"> | ||
| <input_port default="offer" name="pose"/> | ||
| </Action> | ||
| <Action ID="Speak"> | ||
| <input_port name="param"/> | ||
| <input_port name="say_text"/> | ||
| </Action> | ||
| </TreeNodesModel> | ||
| <!-- ////////// --> | ||
| </root> |
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,12 @@ | ||
| <?xml version="1.0"?> | ||
| <root main_tree_to_execute="BehaviorTree"> | ||
| <!-- ////////// --> | ||
| <BehaviorTree ID="BehaviorTree"> | ||
| <Action ID="InitCarry"/> | ||
| </BehaviorTree> | ||
| <!-- ////////// --> | ||
| <TreeNodesModel> | ||
| <Action ID="InitCarry"/> | ||
| </TreeNodesModel> | ||
| <!-- ////////// --> | ||
| </root> |
Oops, something went wrong.