Visualized Translated pose seems legit

Author: suchkristenwow

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,52 @@
+#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,29 @@
+<?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,59 @@
+#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,38 @@
+#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_transform_stamped/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.22)
+project(convert_pose_stamped_to_transform_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_pose_stamped_to_transform_stamped
+  SHARED
+  src/convert_pose_stamped_to_transform_stamped.cpp
+  src/register_behaviors.cpp)
+target_include_directories(
+  convert_pose_stamped_to_transform_stamped
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(convert_pose_stamped_to_transform_stamped
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Install Libraries
+install(
+  TARGETS convert_pose_stamped_to_transform_stamped
+  EXPORT convert_pose_stamped_to_transform_stampedTargets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+if(BUILD_TESTING)
+  moveit_pro_behavior_test(convert_pose_stamped_to_transform_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_pose_stamped_to_transform_stamped_plugin_description.xml)
+
+ament_export_targets(convert_pose_stamped_to_transform_stampedTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()