yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Humble merge

Browse Source
This commit is contained in:
Bastian Hofmann 2022-11-22 15:27:05 +01:00
parent 9c0c189a23
commit 36c235b781
13 changed files with 111 additions and 242 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/btree_nodes/CMakeLists.txt
View File

@ -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})

28
src/btree_nodes/src/Extensions.cpp
View File

@ -50,13 +50,13 @@ namespace BT {
}
auto pose = std::make_shared<geometry_msgs::msg::Pose>();
pose->orientation.w = 0;
pose->orientation.x = 0;
pose->orientation.y = 1;
pose->orientation.z = 0;
pose->position.x = convertFromString<double>(parts[0]);
pose->position.y = convertFromString<double>(parts[1]);
pose->position.z = convertFromString<double>(parts[2]);
pose->orientation.w=0;
pose->orientation.x=0;
pose->orientation.y=1;
pose->orientation.z=0;
pose->position.x= convertFromString<double>(parts[0]);
pose->position.y= convertFromString<double>(parts[1]);
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->y = convertFromString<double>(parts[1]);
point->z = convertFromString<double>(parts[2]);
point->x= convertFromString<double>(parts[0]);
point->y= convertFromString<double>(parts[1]);
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->x = convertFromString<double>(parts[1]);
point->y = convertFromString<double>(parts[2]);
point->z = convertFromString<double>(parts[3]);
point->w= convertFromString<double>(parts[0]);
point->x= convertFromString<double>(parts[1]);
point->y= convertFromString<double>(parts[2]);
point->z= convertFromString<double>(parts[3]);
return point;
}

79
src/btree_robot/CMakeLists.txt
View File

@ -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()

19
src/btree_robot/package.xml
View File

@ -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>

88
src/btree_robot/src/robot_test.cpp
View File

@ -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;
}

31
src/ign_actor_plugin/CMakeLists.txt
View File

@ -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}
)

9
src/ign_actor_plugin/package.xml
View File

@ -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>

33
src/ign_actor_plugin/src/ActorSystem.cpp
View File

@ -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
@ -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<const sdf::Element> &_sdf,
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, {});
@ -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 ){}

18
src/ign_actor_plugin/src/ActorSystem.h → src/ign_actor_plugin/src/ActorSystem.hpp
View File

@ -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
View 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
View 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
View 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
View 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>