Humble merge

src/btree_nodes/CMakeLists.txt

@@ -1,12 +1,12 @@
-cmake_minimum_required(VERSION VERSION 3.5.1)
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 project(btree_nodes)
 
-set(CMAKE_CXX_STANDARD 20)
 add_compile_options(-Wall -Wextra -Wpedantic)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 
+
 # find dependencies
 
 set(DEPENDENCIES
@@ -51,7 +51,9 @@ set(CPP_FILES
         )
 
 add_library(tree_plugins_base src/Factory.cpp)
+set_property(TARGET tree_plugins_base PROPERTY CXX_STANDARD 17)
 add_executable(tree ${CPP_FILES})
+set_property(TARGET tree PROPERTY CXX_STANDARD 17)
 ament_target_dependencies(tree_plugins_base ${DEPENDENCIES})
 ament_target_dependencies(tree ${DEPENDENCIES})
 

src/btree_nodes/src/Extensions.cpp

@@ -50,13 +50,13 @@ namespace BT {
         }
 
         auto pose = std::make_shared<geometry_msgs::msg::Pose>();
-        pose->orientation.w = 0;
+        pose->orientation.w=0;
-        pose->orientation.x = 0;
+        pose->orientation.x=0;
-        pose->orientation.y = 1;
+        pose->orientation.y=1;
-        pose->orientation.z = 0;
+        pose->orientation.z=0;
-        pose->position.x = convertFromString<double>(parts[0]);
+        pose->position.x= convertFromString<double>(parts[0]);
-        pose->position.y = convertFromString<double>(parts[1]);
+        pose->position.y= convertFromString<double>(parts[1]);
-        pose->position.z = convertFromString<double>(parts[2]);
+        pose->position.z= convertFromString<double>(parts[2]);
 
         std::cout << "[ generated pose from string ]" << std::endl;
 
@@ -71,9 +71,9 @@ namespace BT {
         }
 
         auto point = std::make_shared<geometry_msgs::msg::Point>();
-        point->x = convertFromString<double>(parts[0]);
+        point->x= convertFromString<double>(parts[0]);
-        point->y = convertFromString<double>(parts[1]);
+        point->y= convertFromString<double>(parts[1]);
-        point->z = convertFromString<double>(parts[2]);
+        point->z= convertFromString<double>(parts[2]);
 
         return point;
     }
@@ -86,10 +86,10 @@ namespace BT {
         }
 
         auto point = std::make_shared<geometry_msgs::msg::Quaternion>();
-        point->w = convertFromString<double>(parts[0]);
+        point->w= convertFromString<double>(parts[0]);
-        point->x = convertFromString<double>(parts[1]);
+        point->x= convertFromString<double>(parts[1]);
-        point->y = convertFromString<double>(parts[2]);
+        point->y= convertFromString<double>(parts[2]);
-        point->z = convertFromString<double>(parts[3]);
+        point->z= convertFromString<double>(parts[3]);
 
         return point;
     }

src/btree_robot/CMakeLists.txt

@@ -1,79 +0,0 @@
-cmake_minimum_required(VERSION VERSION 3.5.1)
-project(btree_robot)
-
-set(CMAKE_CXX_STANDARD 20)
-add_compile_options(-Wall -Wextra -Wpedantic)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-
-set(THIS_PACKAGE_INCLUDE_DEPENDS
-  ament_cmake
-  rclcpp
-  rclcpp_action
-  tf2_geometry_msgs
-#  tf2_ros
-  moveit_core
-#  rviz_visual_tools
-#  moveit_visual_tools
-  moveit_ros_planning_interface
-#  interactive_markers
-  tf2_geometry_msgs
-  moveit_ros_planning
-#  pluginlib
-#  Eigen3
-#  Boost
-#  control_msgs
-#  moveit_servo
-)
-
-#find_package(Eigen3 REQUIRED)
-#find_package(Boost REQUIRED system filesystem date_time thread)
-find_package(ament_cmake REQUIRED)
-#find_package(control_msgs REQUIRED)
-find_package(moveit_core REQUIRED)
-find_package(moveit_ros_planning REQUIRED)
-find_package(moveit_ros_planning_interface REQUIRED)
-#find_package(moveit_ros_perception REQUIRED)
-#find_package(moveit_servo REQUIRED)
-#find_package(interactive_markers REQUIRED)
-#find_package(rviz_visual_tools REQUIRED)
-#find_package(moveit_visual_tools REQUIRED)
-#find_package(geometric_shapes REQUIRED)
-#find_package(pcl_ros REQUIRED)
-#find_package(pcl_conversions REQUIRED)
-#find_package(rosbag REQUIRED)
-find_package(rclcpp REQUIRED)
-#find_package(rclcpp_action REQUIRED)
-#find_package(pluginlib REQUIRED)
-#find_package(tf2_ros REQUIRED)
-#find_package(tf2_eigen REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-
-# find dependencies
-
-find_package(std_msgs REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
-
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-add_executable(tree src/robot_test.cpp)
-ament_target_dependencies(tree ${THIS_PACKAGE_INCLUDE_DEPENDS} std_msgs behaviortree_cpp_v3)
-
-install(TARGETS
-  tree
-  DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()

src/btree_robot/package.xml

@@ -1,19 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>btree_robot</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="bastian@todo.todo">bastian</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <depend>rclcpp</depend>
-  <depend>geometry_msgs</depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/btree_robot/src/robot_test.cpp

@@ -1,88 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2013, SRI International
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of SRI International nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */
-
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-// All source files that use ROS logging should define a file-specific
-// static const rclcpp::Logger named LOGGER, located at the top of the file
-// and inside the namespace with the narrowest scope (if there is one)
-//static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");
-
-int main(int argc, char **argv) {
-    int i;
-    printf("%d\n", argc);
-    for (i = 0; i < argc - 1; i++) {
-        printf("%s\n", argv[i]);
-    }
-
-    rclcpp::init(argc, argv);
-    rclcpp::NodeOptions node_options;
-    node_options.automatically_declare_parameters_from_overrides(true);
-    auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);
-
-    rclcpp::executors::SingleThreadedExecutor executor;
-    executor.add_node(move_group_node);
-    std::thread([&executor]() { executor.spin(); }).detach();
-
-    std::string PLANNING_GROUP = "iisy";
-    moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
-
-    geometry_msgs::msg::Pose target_pose1;
-    target_pose1.orientation.w = 0;
-    target_pose1.orientation.y = 1;
-
-    target_pose1.position.x = 0.2;
-    target_pose1.position.y = -0.2;
-    target_pose1.position.z = 1.2;
-
-
-    // Now, we call the planner to compute the plan and visualize it.
-    // Note that we are just planning, not asking move_group
-    // to actually move the robot.
-
-    //RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
-
-    std::thread([&move_group, &target_pose1]() {
-        move_group.setPoseTarget(target_pose1);
-        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-        bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-        move_group.execute(my_plan);
-    }).join();
-
-    rclcpp::shutdown();
-    return 0;
-}

src/ign_actor_plugin/CMakeLists.txt

@@ -1,36 +1,31 @@
-cmake_minimum_required(VERSION 3.5.1)
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
 project(ign_actor_plugin)
 
-set(IGN_PLUGIN_VER 0)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(ignition-cmake2 REQUIRED)
-find_package(ignition-gazebo5 REQUIRED)
+find_package(ignition-gazebo6 REQUIRED)
+find_package(ign_actor_plugin_msgs REQUIRED)
 
-find_package(rosidl_default_generators REQUIRED)
+find_package(ignition-plugin1 REQUIRED COMPONENTS register)
-
-ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register)
 set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
 
-rosidl_generate_interfaces(${PROJECT_NAME}
+ament_export_dependencies(ActorPlugin 
-        "action/Animation.action"
+        "rosidl_default_runtime"
-        "action/Movement.action"
+        "ign_actor_plugin_msgs"
-        )
+        "rclcpp"
-
+        "rclcpp_action"
-ament_export_dependencies(rosidl_default_runtime)
+        "rclcpp_components")
 
 # Add sources for each plugin to be registered.
-add_library(ActorPlugin src/ActorSystem.cpp)
+add_library(ActorPlugin SHARED src/ActorSystem.cpp)
-ament_target_dependencies(ActorPlugin rclcpp)
+ament_target_dependencies(ActorPlugin rclcpp rclcpp_action ign_actor_plugin_msgs)
 set_property(TARGET ActorPlugin PROPERTY CXX_STANDARD 17)
 
-rosidl_target_interfaces(ActorPlugin
-        ${PROJECT_NAME} "rosidl_typesupport_cpp")
-
 target_link_libraries(ActorPlugin
+        ignition-gazebo6::ignition-gazebo6
         ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
-        ignition-gazebo5::ignition-gazebo5
         ${rclcpp_LIBRARIES}
         ${rclcpp_action_LIBRARIES}
         )

src/ign_actor_plugin/package.xml

@@ -8,20 +8,15 @@
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>rclcpp_components</depend>
-  <depend>ign_actor_plugin_messages</depend>
+  <depend>ign_actor_plugin_msgs</depend>
-  <depend>action_msgs</depend>
   <depend>ignition-cmake2</depend>
-  <depend>ignition-gazebo5</depend>
+  <depend>ignition-gazebo6</depend>
-  <exec_depend>rosidl_default_runtime</exec_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
-  <member_of_group>rosidl_interface_packages</member_of_group>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -2,27 +2,28 @@
 // Created by bastian on 31.08.22.
 //
 
+#include <ignition/gazebo/Entity.hh>
+#include <ignition/gazebo/Types.hh>
 #include <rclcpp_action/create_server.hpp>
-#include "ActorSystem.h"
+#include <rclcpp_action/server.hpp>
+#include "ActorSystem.hpp"
 
-#define AnimationGoalPtr const std::shared_ptr<const ign_actor_plugin::action::Animation::Goal>
-#define AnimationServerGoalPtr const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin::action::Animation>>&
-
-#define MovementGoalPtr const std::shared_ptr<const ign_actor_plugin::action::Movement::Goal>
-#define MovementServerGoalPtr const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin::action::Movement>>&
 
 IGNITION_ADD_PLUGIN(
         ActorSystem,
         ignition::gazebo::System,
         ActorSystem::ISystemPreUpdate,
-        ActorSystem::ISystemConfigure)
+        ActorSystem::ISystemConfigure);
 
 ActorSystem::ActorSystem() = default;
 
 ActorSystem::~ActorSystem() = default;
 
+
+
 void ActorSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) {
-    //_ecm.EachNew<>()
+
+    
 }
 
 // Found too late: https://github.com/AlanSixth/gazebo_ros_action_tutorial/blob/master/src/gazebo_ros_action_tutorial.cc
@@ -31,6 +32,14 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
                             ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &) {
     printf("Plugin configuring...\n");
     
+    
+    /*auto actorComp = _ecm.Component<Actor>(_entity);
+  if (!actorComp)
+  {
+    gzerr << "Entity [" << _entity << "] is not an actor." << std::endl;
+    return;
+  }*/
+
     rclcpp::init(0, {});
 
     std::string topic = "ActorPlugin";
@@ -44,12 +53,14 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
 
     node = rclcpp::Node::make_shared("moveService", topic);
 
-    animationServer = rclcpp_action::create_server<ign_actor_plugin::action::Animation>(
+
+    animationServer = rclcpp_action::create_server<ign_actor_plugin_msgs::action::Animation>(
             node,
             "animation",
             [](rclcpp_action::GoalUUID goalUuid,
                 AnimationGoalPtr &animationGoal) {
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+                return rclcpp_action::GoalResponse::REJECT;
             },
             [](AnimationServerGoalPtr PH1) {
                 return rclcpp_action::CancelResponse::ACCEPT;
@@ -59,13 +70,15 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
             }
     );
 
-    movementServer = rclcpp_action::create_server<ign_actor_plugin::action::Movement>(
+    movementServer = rclcpp_action::create_server<ign_actor_plugin_msgs::action::Movement>(
             node,
             "movement",
             [](rclcpp_action::GoalUUID goalUuid, MovementGoalPtr &movementGoal ){
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+                
                 },
             [](MovementServerGoalPtr goalUuid ){
+                
                 return rclcpp_action::CancelResponse::ACCEPT;
                 },
             [](MovementServerGoalPtr goalUuid ){}

src/ign_actor_plugin/src/ActorSystem.hpp

@@ -7,10 +7,18 @@
 
 #include <ignition/gazebo/System.hh>
 #include <ignition/plugin/Register.hh>
+#include <queue>
 #include <rclcpp/node.hpp>
-#include "rclcpp/rclcpp.hpp"
+#include <rclcpp/rclcpp.hpp>
-#include "ign_actor_plugin/action/animation.hpp"
+#include <rclcpp_action/server.hpp>
-#include "ign_actor_plugin/action/movement.hpp"
+#include <ign_actor_plugin_msgs/action/animation.hpp>
+#include <ign_actor_plugin_msgs/action/movement.hpp>
+
+#define AnimationGoalPtr const std::shared_ptr<const ign_actor_plugin_msgs::action::Animation::Goal>
+#define AnimationServerGoalPtr const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin_msgs::action::Animation>>&
+
+#define MovementGoalPtr const std::shared_ptr<const ign_actor_plugin_msgs::action::Movement::Goal>
+#define MovementServerGoalPtr const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin_msgs::action::Movement>>&
 
 class ActorSystem:
         public ignition::gazebo::System,
@@ -19,8 +27,8 @@ class ActorSystem:
 
 private:
     std::shared_ptr<rclcpp::Node> node;
-    std::shared_ptr<rclcpp_action::Server<ign_actor_plugin::action::Animation>> animationServer;
+    std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Animation>> animationServer;
-    std::shared_ptr<rclcpp_action::Server<ign_actor_plugin::action::Movement>> movementServer;
+    std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Movement>> movementServer;
 
 public:
     ActorSystem();

src/ign_actor_plugin_msgs/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+project(ign_actor_plugin_msgs)
+
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "action/Animation.action"
+        "action/Movement.action"
+        )
+
+ament_package()

src/ign_actor_plugin_msgs/action/Animation.action

@@ -0,0 +1,6 @@
+string animation_name
+float32 animation_speed
+---
+bool success
+---
+float32 progress

src/ign_actor_plugin_msgs/action/Movement.action

@@ -0,0 +1,8 @@
+string animation_name
+float32 animation_speed
+float32[3] target_position
+float32[3] target_orientation
+---
+bool success
+---
+float32 progress

src/ign_actor_plugin_msgs/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ign_actor_plugin_msgs</name>
+  <version>0.0.0</version>
+  <description>Plugin for Gazebo Ignition to remote control actors</description>
+  <maintainer email="bastian@todo.todo">Bastian Hofmann</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <depend>action_msgs</depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>