Teleop with quest v2

.gitignore

@@ -2,3 +2,4 @@ build/
 install/
 log/
 .vscode/
+src/ROS-TCP-Endpoint/

src/convert_odom_to_pose_stamped/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.22)
+project(convert_odom_to_pose_stamped CXX)
+
+find_package(moveit_pro_package REQUIRED)
+moveit_pro_package()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib)
+foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${package} REQUIRED)
+endforeach()
+
+add_library(
+  convert_odom_to_pose_stamped
+  SHARED
+  src/convert_odom_to_pose_stamped.cpp
+  src/register_behaviors.cpp)
+target_include_directories(
+  convert_odom_to_pose_stamped
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(convert_odom_to_pose_stamped
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Install Libraries
+install(
+  TARGETS convert_odom_to_pose_stamped
+  EXPORT convert_odom_to_pose_stampedTargets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+if(BUILD_TESTING)
+  moveit_pro_behavior_test(convert_odom_to_pose_stamped)
+endif()
+
+# Export the behavior plugins defined in this package so they are available to
+# plugin loaders that load the behavior base class library from the
+# moveit_studio_behavior package.
+pluginlib_export_plugin_description_file(
+  moveit_pro_behavior_interface convert_odom_to_pose_stamped_plugin_description.xml)
+
+ament_export_targets(convert_odom_to_pose_stampedTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/convert_odom_to_pose_stamped/behavior_plugin.yaml

@@ -0,0 +1,4 @@
+objectives:
+  behavior_loader_plugins:
+    convert_odom_to_pose_stamped:
+      - "convert_odom_to_pose_stamped::ConvertOdomToPoseStampedBehaviorsLoader"

src/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped_plugin_description.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<library path="convert_odom_to_pose_stamped">
+  <class
+    type="convert_odom_to_pose_stamped::ConvertOdomToPoseStampedBehaviorsLoader"
+    base_class_type="moveit_pro::behaviors::SharedResourcesNodeLoaderBase"
+  />
+</library>

src/convert_odom_to_pose_stamped/include/convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+#include <moveit_pro_behavior_interface/get_required_ports.hpp>
+
+
+
+
+
+namespace convert_odom_to_pose_stamped
+{
+/**
+ * @brief TODO(...)
+ */
+class ConvertOdomToPoseStamped : public BT::SyncActionNode
+{
+public:
+  /**
+   * @brief Constructor for the convert_odom_to_pose_stamped behavior.
+   * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree.
+   * @param config This contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree.
+   * @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor.
+   */
+  ConvertOdomToPoseStamped(const std::string& name, const BT::NodeConfiguration& config);
+
+
+  /**
+   * @brief Implementation of the required providedPorts() function for the convert_odom_to_pose_stamped Behavior.
+   * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function must return an empty BT::PortsList.
+   * This function returns a list of ports with their names and port info, which is used internally by the behavior tree.
+   * @return convert_odom_to_pose_stamped does not expose any ports, so this function returns an empty list.
+   */
+  static BT::PortsList providedPorts();
+
+  /**
+   * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+   * subcategory, in the MoveIt Studio Developer Tool.
+   * @return A BT::KeyValueVector containing the Behavior metadata.
+   */
+  static BT::KeyValueVector metadata();
+
+  /**
+   * @brief Implementation of BT::SyncActionNode::tick() for ConvertOdomToPoseStamped.
+   * @details This function is where the Behavior performs its work when the behavior tree is being run. Since ConvertOdomToPoseStamped is derived from BT::SyncActionNode, it is very important that its tick() function always finishes very quickly. If tick() blocks before returning, it will block execution of the entire behavior tree, which may have undesirable consequences for other Behaviors that require a fast update rate to work correctly.
+   */
+  BT::NodeStatus tick() override;
+
+};
+}  // namespace convert_odom_to_pose_stamped

src/convert_odom_to_pose_stamped/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<package format="3">
+  <name>convert_odom_to_pose_stamped</name>
+  <version>0.0.0</version>
+  <description>This removes the twist part of the odom message so you can visualize/operate on the pose. </description>
+
+  <maintainer email="support@picknik.ai">
+    MoveIt Pro User
+  </maintainer>
+  <author email="support@picknik.ai">
+    MoveIt Pro User
+  </author>
+
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>moveit_pro_package</build_depend>
+
+  <depend>moveit_pro_behavior_interface</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+  <buildtool_depend>python3-colcon-common-extensions</buildtool_depend>
+</package>

src/convert_odom_to_pose_stamped/src/convert_odom_to_pose_stamped.cpp

@@ -0,0 +1,62 @@
+#include <convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp>
+
+#include "spdlog/spdlog.h"
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp> 
+
+
+namespace
+{
+constexpr auto kPortIDOdometry = "odometry";
+constexpr auto kPortIDPoseStamped = "pose_stamped";
+}  // namespace
+
+namespace convert_odom_to_pose_stamped
+{
+ConvertOdomToPoseStamped::ConvertOdomToPoseStamped(const std::string& name, const BT::NodeConfiguration& config)
+  : BT::SyncActionNode(name, config)
+{
+}
+
+
+BT::PortsList ConvertOdomToPoseStamped::providedPorts()
+{
+  return BT::PortsList(
+      { BT::InputPort<nav_msgs::msg::Odometry>(kPortIDOdometry, "{odometry}",
+                                                            "The gnav_msgs::msg::Odometry message to "
+                                                            "convert."),
+        BT::OutputPort<geometry_msgs::msg::PoseStamped>(kPortIDPoseStamped, "{pose_stamped}",
+                                                        "The converted geometry_msgs::msg::PoseStamped message.") });
+}
+
+BT::KeyValueVector ConvertOdomToPoseStamped::metadata()
+{
+  return { { "description", "Converts a nav_msgs::msg::Odometry message into a "
+                            "geometry_msgs::msg::PoseStamped message." },
+           { "subcategory", "Conversions" } };
+}
+
+BT::NodeStatus ConvertOdomToPoseStamped::tick()
+{
+  const auto ports = moveit_pro::behaviors::getRequiredInputs(
+      getInput<nav_msgs::msg::Odometry>(kPortIDOdometry));
+
+  if (!ports.has_value())
+  {
+    spdlog::warn("Failed to get required value from input data port: {}", ports.error());
+    return BT::NodeStatus::FAILURE;
+  }
+
+  const auto& odom_msg = std::get<0>(ports.value());
+
+  geometry_msgs::msg::PoseStamped pose_out;
+  pose_out.header = odom_msg.header;
+  pose_out.pose = odom_msg.pose.pose; 
+
+  setOutput(kPortIDPoseStamped, pose_out);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace convert_odom_to_pose_stamped

src/convert_odom_to_pose_stamped/src/register_behaviors.cpp

@@ -0,0 +1,24 @@
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit_pro_behavior_interface/behavior_context.hpp>
+#include <moveit_pro_behavior_interface/shared_resources_node_loader.hpp>
+
+#include <convert_odom_to_pose_stamped/convert_odom_to_pose_stamped.hpp>
+
+#include <pluginlib/class_list_macros.hpp>
+
+namespace convert_odom_to_pose_stamped
+{
+class ConvertOdomToPoseStampedBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase
+{
+public:
+  void registerBehaviors(BT::BehaviorTreeFactory& factory,
+    [[maybe_unused]] const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources) override
+  {
+    moveit_pro::behaviors::registerBehavior<ConvertOdomToPoseStamped>(factory, "ConvertOdomToPoseStamped");
+
+  }
+};
+}  // namespace convert_odom_to_pose_stamped
+
+PLUGINLIB_EXPORT_CLASS(convert_odom_to_pose_stamped::ConvertOdomToPoseStampedBehaviorsLoader,
+                       moveit_pro::behaviors::SharedResourcesNodeLoaderBase);

src/convert_odom_to_pose_stamped/test/CMakeLists.txt

@@ -0,0 +1,5 @@
+find_package(ament_cmake_gtest REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+
+ament_add_ros_isolated_gtest(test_behavior_plugins test_behavior_plugins.cpp)
+ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS})

src/convert_odom_to_pose_stamped/test/test_behavior_plugins.cpp

@@ -0,0 +1,37 @@
+#include <gtest/gtest.h>
+
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit_pro_behavior_interface/shared_resources_node_loader.hpp>
+#include <pluginlib/class_loader.hpp>
+#include <rclcpp/node.hpp>
+
+/**
+ * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and
+ * instantiated by the behavior tree factory.
+ */
+TEST(BehaviorTests, test_load_behavior_plugins)
+{
+  pluginlib::ClassLoader<moveit_pro::behaviors::SharedResourcesNodeLoaderBase> class_loader(
+      "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase");
+
+  auto node = std::make_shared<rclcpp::Node>("BehaviorTests");
+  auto shared_resources = std::make_shared<moveit_pro::behaviors::BehaviorContext>(node);
+
+  BT::BehaviorTreeFactory factory;
+  {
+    auto plugin_instance = class_loader.createUniqueInstance("convert_odom_to_pose_stamped::ConvertOdomToPoseStampedBehaviorsLoader");
+    ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources));
+  }
+
+  // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info.
+  EXPECT_NO_THROW(
+    (void)factory.instantiateTreeNode("test_behavior_name", "ConvertOdomToPoseStamped", BT::NodeConfiguration()));
+}
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/convert_pose_stamped_to_odom/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.22)
+project(convert_pose_stamped_to_odom CXX)
+
+find_package(moveit_pro_package REQUIRED)
+moveit_pro_package()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_pro_behavior_interface pluginlib)
+foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${package} REQUIRED)
+endforeach()
+
+add_library(
+  convert_pose_stamped_to_odom
+  SHARED
+  src/convert_pose_stamped_to_odom.cpp
+  src/register_behaviors.cpp)
+target_include_directories(
+  convert_pose_stamped_to_odom
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(convert_pose_stamped_to_odom
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Install Libraries
+install(
+  TARGETS convert_pose_stamped_to_odom
+  EXPORT convert_pose_stamped_to_odomTargets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+if(BUILD_TESTING)
+  moveit_pro_behavior_test(convert_pose_stamped_to_odom)
+endif()
+
+# Export the behavior plugins defined in this package so they are available to
+# plugin loaders that load the behavior base class library from the
+# moveit_studio_behavior package.
+pluginlib_export_plugin_description_file(
+  moveit_pro_behavior_interface convert_pose_stamped_to_odom_plugin_description.xml)
+
+ament_export_targets(convert_pose_stamped_to_odomTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/convert_pose_stamped_to_odom/behavior_plugin.yaml

@@ -0,0 +1,4 @@
+objectives:
+  behavior_loader_plugins:
+    convert_pose_stamped_to_odom:
+      - "convert_pose_stamped_to_odom::ConvertPoseStampedToOdomBehaviorsLoader"

src/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom_plugin_description.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<library path="convert_pose_stamped_to_odom">
+  <class
+    type="convert_pose_stamped_to_odom::ConvertPoseStampedToOdomBehaviorsLoader"
+    base_class_type="moveit_pro::behaviors::SharedResourcesNodeLoaderBase"
+  />
+</library>

src/convert_pose_stamped_to_odom/include/convert_pose_stamped_to_odom/convert_pose_stamped_to_odom.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+#include <moveit_pro_behavior_interface/get_required_ports.hpp>
+
+#include <behaviortree_cpp/action_node.h>
+#include <behaviortree_cpp/basic_types.h>   // BT::PortsList, BT::KeyValueVector
+
+namespace convert_pose_stamped_to_odom
+{
+/**
+ * @brief TODO(...)
+ */
+class ConvertPoseStampedToOdom : public BT::SyncActionNode
+{
+public:
+  ConvertPoseStampedToOdom(const std::string& name,
+                           const BT::NodeConfiguration& config);
+
+  static BT::PortsList providedPorts();
+  static BT::KeyValueVector metadata();
+
+  BT::NodeStatus tick() override;
+};
+
+}  // namespace convert_pose_stamped_to_odom

src/convert_pose_stamped_to_odom/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<package format="3">
+  <name>convert_pose_stamped_to_odom</name>
+  <version>0.0.0</version>
+  <description>This converts a pose stamped message to an odometry message (leaves the twist part 0).</description>
+
+  <maintainer email="support@picknik.ai">
+    MoveIt Pro User
+  </maintainer>
+  <author email="support@picknik.ai">
+    MoveIt Pro User
+  </author>
+
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>moveit_pro_package</build_depend>
+
+  <depend>moveit_pro_behavior_interface</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+  <buildtool_depend>python3-colcon-common-extensions</buildtool_depend>
+</package>