diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index ab869d2..46bb692 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/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}) diff --git a/src/btree_nodes/src/Extensions.cpp b/src/btree_nodes/src/Extensions.cpp index 730dbbd..7e4eb1e 100644 --- a/src/btree_nodes/src/Extensions.cpp +++ b/src/btree_nodes/src/Extensions.cpp @@ -50,13 +50,13 @@ namespace BT { } auto pose = std::make_shared(); - pose->orientation.w = 0; - pose->orientation.x = 0; - pose->orientation.y = 1; - pose->orientation.z = 0; - pose->position.x = convertFromString(parts[0]); - pose->position.y = convertFromString(parts[1]); - pose->position.z = convertFromString(parts[2]); + pose->orientation.w=0; + pose->orientation.x=0; + pose->orientation.y=1; + pose->orientation.z=0; + pose->position.x= convertFromString(parts[0]); + pose->position.y= convertFromString(parts[1]); + pose->position.z= convertFromString(parts[2]); std::cout << "[ generated pose from string ]" << std::endl; @@ -71,9 +71,9 @@ namespace BT { } auto point = std::make_shared(); - point->x = convertFromString(parts[0]); - point->y = convertFromString(parts[1]); - point->z = convertFromString(parts[2]); + point->x= convertFromString(parts[0]); + point->y= convertFromString(parts[1]); + point->z= convertFromString(parts[2]); return point; } @@ -86,10 +86,10 @@ namespace BT { } auto point = std::make_shared(); - point->w = convertFromString(parts[0]); - point->x = convertFromString(parts[1]); - point->y = convertFromString(parts[2]); - point->z = convertFromString(parts[3]); + point->w= convertFromString(parts[0]); + point->x= convertFromString(parts[1]); + point->y= convertFromString(parts[2]); + point->z= convertFromString(parts[3]); return point; } diff --git a/src/btree_robot/CMakeLists.txt b/src/btree_robot/CMakeLists.txt deleted file mode 100644 index c8303ad..0000000 --- a/src/btree_robot/CMakeLists.txt +++ /dev/null @@ -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( 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() diff --git a/src/btree_robot/package.xml b/src/btree_robot/package.xml deleted file mode 100644 index a8fb8fe..0000000 --- a/src/btree_robot/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - btree_robot - 0.0.0 - TODO: Package description - bastian - TODO: License declaration - - ament_cmake - rclcpp - geometry_msgs - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/btree_robot/src/robot_test.cpp b/src/btree_robot/src/robot_test.cpp deleted file mode 100644 index 70d639f..0000000 --- a/src/btree_robot/src/robot_test.cpp +++ /dev/null @@ -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 -#include - -// 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; -} diff --git a/src/ign_actor_plugin/CMakeLists.txt b/src/ign_actor_plugin/CMakeLists.txt index 6d8a08c..8ba3cb2 100644 --- a/src/ign_actor_plugin/CMakeLists.txt +++ b/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) - -ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) +find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) -rosidl_generate_interfaces(${PROJECT_NAME} - "action/Animation.action" - "action/Movement.action" - ) - -ament_export_dependencies(rosidl_default_runtime) +ament_export_dependencies(ActorPlugin + "rosidl_default_runtime" + "ign_actor_plugin_msgs" + "rclcpp" + "rclcpp_action" + "rclcpp_components") # Add sources for each plugin to be registered. -add_library(ActorPlugin src/ActorSystem.cpp) -ament_target_dependencies(ActorPlugin rclcpp) +add_library(ActorPlugin SHARED src/ActorSystem.cpp) +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} ) diff --git a/src/ign_actor_plugin/package.xml b/src/ign_actor_plugin/package.xml index 31a2061..24b77b1 100644 --- a/src/ign_actor_plugin/package.xml +++ b/src/ign_actor_plugin/package.xml @@ -8,20 +8,15 @@ TODO: License declaration ament_cmake - rosidl_default_generators rclcpp rclcpp_action rclcpp_components - ign_actor_plugin_messages - action_msgs + ign_actor_plugin_msgs ignition-cmake2 - ignition-gazebo5 - rosidl_default_runtime + ignition-gazebo6 ament_lint_auto ament_lint_common - rosidl_interface_packages - ament_cmake diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp index 899ff50..08f6d91 100644 --- a/src/ign_actor_plugin/src/ActorSystem.cpp +++ b/src/ign_actor_plugin/src/ActorSystem.cpp @@ -2,27 +2,28 @@ // Created by bastian on 31.08.22. // +#include +#include #include -#include "ActorSystem.h" +#include +#include "ActorSystem.hpp" -#define AnimationGoalPtr const std::shared_ptr -#define AnimationServerGoalPtr const std::shared_ptr>& - -#define MovementGoalPtr const std::shared_ptr -#define MovementServerGoalPtr const std::shared_ptr>& 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 @@ -30,6 +31,14 @@ void ActorSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition: void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::shared_ptr &_sdf, ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &) { printf("Plugin configuring...\n"); + + + /*auto actorComp = _ecm.Component(_entity); + if (!actorComp) + { + gzerr << "Entity [" << _entity << "] is not an actor." << std::endl; + return; + }*/ rclcpp::init(0, {}); @@ -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( + + animationServer = rclcpp_action::create_server( 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( + movementServer = rclcpp_action::create_server( node, "movement", [](rclcpp_action::GoalUUID goalUuid, MovementGoalPtr &movementGoal ){ return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, [](MovementServerGoalPtr goalUuid ){ + return rclcpp_action::CancelResponse::ACCEPT; }, [](MovementServerGoalPtr goalUuid ){} diff --git a/src/ign_actor_plugin/src/ActorSystem.h b/src/ign_actor_plugin/src/ActorSystem.hpp similarity index 52% rename from src/ign_actor_plugin/src/ActorSystem.h rename to src/ign_actor_plugin/src/ActorSystem.hpp index 69f2795..cfd659a 100644 --- a/src/ign_actor_plugin/src/ActorSystem.h +++ b/src/ign_actor_plugin/src/ActorSystem.hpp @@ -7,10 +7,18 @@ #include #include +#include #include -#include "rclcpp/rclcpp.hpp" -#include "ign_actor_plugin/action/animation.hpp" -#include "ign_actor_plugin/action/movement.hpp" +#include +#include +#include +#include + +#define AnimationGoalPtr const std::shared_ptr +#define AnimationServerGoalPtr const std::shared_ptr>& + +#define MovementGoalPtr const std::shared_ptr +#define MovementServerGoalPtr const std::shared_ptr>& class ActorSystem: public ignition::gazebo::System, @@ -19,8 +27,8 @@ class ActorSystem: private: std::shared_ptr node; - std::shared_ptr> animationServer; - std::shared_ptr> movementServer; + std::shared_ptr> animationServer; + std::shared_ptr> movementServer; public: ActorSystem(); diff --git a/src/ign_actor_plugin_msgs/CMakeLists.txt b/src/ign_actor_plugin_msgs/CMakeLists.txt new file mode 100644 index 0000000..f1e43dc --- /dev/null +++ b/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() diff --git a/src/ign_actor_plugin_msgs/action/Animation.action b/src/ign_actor_plugin_msgs/action/Animation.action new file mode 100644 index 0000000..a3b9dc8 --- /dev/null +++ b/src/ign_actor_plugin_msgs/action/Animation.action @@ -0,0 +1,6 @@ +string animation_name +float32 animation_speed +--- +bool success +--- +float32 progress diff --git a/src/ign_actor_plugin_msgs/action/Movement.action b/src/ign_actor_plugin_msgs/action/Movement.action new file mode 100644 index 0000000..664f850 --- /dev/null +++ b/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 diff --git a/src/ign_actor_plugin_msgs/package.xml b/src/ign_actor_plugin_msgs/package.xml new file mode 100644 index 0000000..2b5168d --- /dev/null +++ b/src/ign_actor_plugin_msgs/package.xml @@ -0,0 +1,17 @@ + + + + ign_actor_plugin_msgs + 0.0.0 + Plugin for Gazebo Ignition to remote control actors + Bastian Hofmann + TODO: License declaration + + rosidl_default_generators + action_msgs + rosidl_interface_packages + + + ament_cmake + +