pal integration of face detect

README.md

@@ -2,73 +2,38 @@
 
 ## Installation
 
-See [Software setup](https://github.com/CoreSenseEU/CoreSense4Home/wiki/C-Software-Setup)
+1. Create a dedicated workspace and clone this repo in it
 
-
-## Usage 
-### Navigation
-```bash
-ros2 launch robocup_bringup navigation.launch.py
 ```
-### Launch current carry my luggage implementation
+mkdir -p robocup/src
+cd robocup/src
+git clone https://github.com/CoreSenseEU/CoreSense4Home.git
+```
 
-First kill move_group node inside tiago robot. Then in separate terminals launch:
+2. Install [Whisper_ROS](https://github.com/mgonzs13/whisper_ros)
 
-```bash
-ros2 launch robocup_bringup navigation_follow.launch.py rviz:=True
-```
-```bash
-ros2 launch attention_system attention.launch.py
-```
-```bash
-ros2 launch robocup_bringup carry_my_luggage_dependencies.launch.py
-``` 
-```bash
-ros2 launch whisper_bringup whisper.launch.py
-```
-Finally:
+   1. [Install CUDA (V12.1)](https://docs.nvidia.com/cuda/cuda-installation-guide-linux/index.html#ubuntu)
+   2. Follow instructions on [whisper_ros repo](https://github.com/mgonzs13/whisper_ros)
 
-```bash
-ros2 run bt_test carry_my_luggage_test
-```
+3. Install internal dependencies
 
-### Follow navigation with small objects
-```bash
-ros2 launch robocup_bringup navigation_follow.launch.py
 ```
-### Demo moveit
-inside tiago, first kill move_group and then:
-```bash
-ros2 launch tiago_moveit_config move_group.launch.py
-```
-Launch the percetion system with the remaps for the tiago, and activate the object detection node
-```bash
-ros2 launch perception_system perception3d.launch.py
-```
-launch the speaking system:
-```bash
-ros2 run audio_common tts_node
-ros2 run audio_common audio_player_node
-ros2 launch whisper_bringup whisper.launch.py
-```
-launch the manipulation system:
-```bash
-ros2 launch action_server server.launch.py
+robocup/src $ vcs import --recursive < CoreSense4Home/robocup_bringup/thirdparty.repos
+robocup/src $ cd ..
+robocup $ rosdep install --from-paths src --ignore-src -r -y
 ```
 
-execute the test:
+## Usage 
 
-```bash
-ros2 run bt_test pick_demo_test
-```
+### Launch current carry my luggage implementation
 
-### Demo Dialog
-```bash
-ros2 launch robocup_bringup dialog.launch.py
-```
+First kill navigation, localization and hri_body_detect (you are going to launch your own) modules inside tiago robot. Then in separate terminals launch:
 
-Execute the test:
+```bash
+ros2 launch robocup_bringup carry_my_luggage_dependencies.launch.py
+``` 
+Finally:
 
 ```bash
-ros2 run bt_test ask_test
+ros2 launch robocup_bringup bt_test carry_my_luggage.launch.py
 ```

bt_nodes/arm/CMakeLists.txt

@@ -11,10 +11,12 @@ find_package(behaviortree_cpp_v3 REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_cascade_lifecycle REQUIRED)
 find_package(rclcpp_action REQUIRED)
-find_package(manipulation_interfaces REQUIRED)
+# find_package(manipulation_interfaces REQUIRED) (juandpenan) todo: uncomment this when pal team finishes testing
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(play_motion2_msgs REQUIRED)
+
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -26,37 +28,43 @@ set(dependencies
   rclcpp
   rclcpp_cascade_lifecycle
   rclcpp_action
-  manipulation_interfaces
+  # manipulation_interfaces (juandpenan) todo: uncomment this when pal team finishes testing
   tf2_ros
   geometry_msgs
   sensor_msgs
+  play_motion2_msgs
 )
 
 include_directories(include)
 
-add_library(pick_bt_node SHARED src/manipulation/pick_object.cpp)
-list(APPEND plugin_libs pick_bt_node)
+# add_library(pick_bt_node SHARED src/manipulation/pick_object.cpp)
+# list(APPEND plugin_libs pick_bt_node)
+
+# add_library(move_to_predefined_bt_node SHARED src/manipulation/move_to_predefined.cpp)
+# list(APPEND plugin_libs move_to_predefined_bt_node)
+
+# add_library(point_at_bt_node SHARED src/manipulation/point_at.cpp)
+# list(APPEND plugin_libs point_at_bt_node)
 
-add_library(move_to_predefined_bt_node SHARED src/manipulation/move_to_predefined.cpp)
-list(APPEND plugin_libs move_to_predefined_bt_node)
+# add_library(place_bt_node SHARED src/manipulation/place_object.cpp)
+# list(APPEND plugin_libs place_bt_node)
 
-add_library(point_at_bt_node SHARED src/manipulation/point_at.cpp)
-list(APPEND plugin_libs point_at_bt_node)
+# add_library(move_joint_bt_node SHARED src/manipulation/move_joint.cpp)
+# list(APPEND plugin_libs move_joint_bt_node)
 
-add_library(place_bt_node SHARED src/manipulation/place_object.cpp)
-list(APPEND plugin_libs place_bt_node)
+# add_library(pick_object_from_pc_bt_node SHARED src/manipulation/pick_object_from_pc.cpp)
+# list(APPEND plugin_libs pick_object_from_pc_bt_node)
 
-add_library(move_joint_bt_node SHARED src/manipulation/move_joint.cpp)
-list(APPEND plugin_libs move_joint_bt_node)
+# add_library(move_end_effector_bt_node SHARED src/manipulation/move_end_effector.cpp)
+# list(APPEND plugin_libs move_end_effector_bt_node)
 
-add_library(pick_object_from_pc_bt_node SHARED src/manipulation/pick_object_from_pc.cpp)
-list(APPEND plugin_libs pick_object_from_pc_bt_node)
+# add_library(generate_grasp_poses_bt_node SHARED src/manipulation/generate_grasp_poses.cpp)
+# list(APPEND plugin_libs generate_grasp_poses_bt_node)
 
-add_library(move_end_effector_bt_node SHARED src/manipulation/move_end_effector.cpp)
-list(APPEND plugin_libs move_end_effector_bt_node)
+# (juandpenan) todo: uncomment this when pal team finishes testing
 
-add_library(generate_grasp_poses_bt_node SHARED src/manipulation/generate_grasp_poses.cpp)
-list(APPEND plugin_libs generate_grasp_poses_bt_node)
+add_library(pal_move_to_predefined_bt_node SHARED src/manipulation/pal_move_to_predefined.cpp)
+list(APPEND plugin_libs pal_move_to_predefined_bt_node)
 
 
 foreach(bt_plugin ${plugin_libs})

bt_nodes/arm/include/arm/manipulation/pal_move_to_predefined.hpp

@@ -0,0 +1,58 @@
+// 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__PAL_MOVE_TO_PREDEFINED_HPP_
+#define ARM_MANIPULATION__PAL_MOVE_TO_PREDEFINED_HPP_
+
+#include <algorithm>
+#include <string>
+
+#include "arm/manipulation/BTActionNode.hpp"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "play_motion2_msgs/action/play_motion2.hpp"
+#include "moveit_msgs/msg/collision_object.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
+namespace manipulation
+{
+
+class PalMoveToPredefined : public manipulation::BtActionNode<
+    play_motion2_msgs::action::PlayMotion2,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+public:
+  explicit PalMoveToPredefined(
+    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"), BT::InputPort<std::string>("group_name")});
+  }
+
+private:
+  std::string group_name_{"arm_torso"};
+
+  std::string pose_;
+};
+
+}  // namespace manipulation
+
+#endif  // ARM_MANIPULATION__PAL_MOVE_TO_PREDEFINED_HPP_

bt_nodes/arm/package.xml

@@ -12,7 +12,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>rclcpp_cascade_lifecycle</depend>
-  <depend>manipulation_interfaces</depend>
+  <!-- <depend>manipulation_interfaces</depend> (juandpenan) uncomment this after pal finishes testing -->
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

bt_nodes/arm/src/manipulation/pal_move_to_predefined.cpp

@@ -0,0 +1,60 @@
+// 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 "arm/manipulation/pal_move_to_predefined.hpp"
+
+#include <string>
+#include <utility>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "play_motion2_msgs/action/play_motion2.hpp"
+
+namespace manipulation
+{
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+PalMoveToPredefined::PalMoveToPredefined(
+  const std::string & xml_tag_name, const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: manipulation::BtActionNode<
+    play_motion2_msgs::action::PlayMotion2,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>(xml_tag_name, action_name, conf)
+{
+}
+
+void PalMoveToPredefined::on_tick()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "PalMoveToPredefined ticked");
+
+  getInput("pose", pose_);
+  goal_.motion_name = pose_;
+}
+
+BT::NodeStatus PalMoveToPredefined::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::PalMoveToPredefined>(name, "/play_motion2", config);
+    };
+
+  factory.registerBuilder<manipulation::PalMoveToPredefined>("PalMoveToPredefined", builder);
+}