Humble merge
This commit is contained in:
Bastian Hofmann
parent
9c0c189a23
commit
36c235b781
@ -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})
|
||||
|
||||
|
||||
@ -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()
|
||||
@ -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>
|
||||
@ -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;
|
||||
}
|
||||
@ -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}
|
||||
)
|
||||
|
||||
@ -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>action_msgs</depend>
|
||||
<depend>ign_actor_plugin_msgs</depend>
|
||||
<depend>ignition-cmake2</depend>
|
||||
<depend>ignition-gazebo5</depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<depend>ignition-gazebo6</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>
|
||||
|
||||
@ -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 ){}
|
||||
|
||||
@ -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 "ign_actor_plugin/action/animation.hpp"
|
||||
#include "ign_actor_plugin/action/movement.hpp"
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/server.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::action::Movement>> movementServer;
|
||||
std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Animation>> animationServer;
|
||||
std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Movement>> movementServer;
|
||||
|
||||
public:
|
||||
ActorSystem();
|
||||
11
src/ign_actor_plugin_msgs/CMakeLists.txt
Normal file
11
src/ign_actor_plugin_msgs/CMakeLists.txt
Normal file
@ -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()
|
||||
6
src/ign_actor_plugin_msgs/action/Animation.action
Normal file
6
src/ign_actor_plugin_msgs/action/Animation.action
Normal file
@ -0,0 +1,6 @@
|
||||
string animation_name
|
||||
float32 animation_speed
|
||||
---
|
||||
bool success
|
||||
---
|
||||
float32 progress
|
||||
8
src/ign_actor_plugin_msgs/action/Movement.action
Normal file
8
src/ign_actor_plugin_msgs/action/Movement.action
Normal file
@ -0,0 +1,8 @@
|
||||
string animation_name
|
||||
float32 animation_speed
|
||||
float32[3] target_position
|
||||
float32[3] target_orientation
|
||||
---
|
||||
bool success
|
||||
---
|
||||
float32 progress
|
||||
17
src/ign_actor_plugin_msgs/package.xml
Normal file
17
src/ign_actor_plugin_msgs/package.xml
Normal file
@ -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>
|
||||
Loading…
x
Reference in New Issue
Block a user