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

Attempt to use new plugin format for tree nodes.

Browse Source 
Crude support for gazebo ignition
This commit is contained in:
Bastian Hofmann 2022-09-12 13:26:25 +02:00
parent 7f9b30083f
commit a7b8389557
10 changed files with 434 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/btree_nodes/CMakeLists.txt
View File

@ -21,6 +21,7 @@ set(DEPENDENCIES
moveit_ros_planning_interface moveit_ros_planning_interface
tf2_geometry_msgs tf2_geometry_msgs
moveit_ros_planning moveit_ros_planning
pluginlib
) )
foreach(dep IN LISTS DEPENDENCIES) foreach(dep IN LISTS DEPENDENCIES)
@ -33,6 +34,7 @@ endforeach()
set(CPP_FILES set(CPP_FILES
src/Tree.cpp src/Tree.cpp
src/Factory.cpp
src/Extensions.cpp src/Extensions.cpp
src/nodes/WeightedRandomNode.cpp src/nodes/WeightedRandomNode.cpp
src/nodes/AmICalled.cpp src/nodes/AmICalled.cpp
@ -48,14 +50,25 @@ set(CPP_FILES
src/nodes/SetRobotVelocity.cpp src/nodes/SetRobotVelocity.cpp
) )
add_library(tree_plugins_base src/Factory.cpp)
add_executable(tree ${CPP_FILES}) add_executable(tree ${CPP_FILES})
ament_target_dependencies(tree_plugins_base ${DEPENDENCIES})
ament_target_dependencies(tree ${DEPENDENCIES}) ament_target_dependencies(tree ${DEPENDENCIES})
pluginlib_export_plugin_description_file(behaviortree_cpp_v3 plugins.xml)
#add_executable(talker src/publisher_member_function.cpp) #add_executable(talker src/publisher_member_function.cpp)
#ament_target_dependencies(talker geometry_msgs rclcpp) #ament_target_dependencies(talker geometry_msgs rclcpp)
#add_executable(listener src/subscriber_member_function.cpp) #add_executable(listener src/subscriber_member_function.cpp)
#ament_target_dependencies(listener geometry_msgs rclcpp) #ament_target_dependencies(listener geometry_msgs rclcpp)
install(TARGETS
tree_plugins_base
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
install(TARGETS install(TARGETS
tree tree
DESTINATION lib/${PROJECT_NAME}) DESTINATION lib/${PROJECT_NAME})
@ -71,4 +84,11 @@ if (BUILD_TESTING)
ament_lint_auto_find_test_dependencies() ament_lint_auto_find_test_dependencies()
endif () endif ()
ament_export_libraries(
tree_plugins_base
)
ament_export_targets(
export_${PROJECT_NAME}
)
ament_package() ament_package()

2
src/btree_nodes/actorTree.xml
View File

@ -9,7 +9,7 @@
</Inverter> </Inverter>
<Sequence> <Sequence>
<Control ID="WeightedRandom" weights="10;2;1"> <Control ID="WeightedRandom" weights="100;5;2">
<Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/> <Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/> <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/> <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>

5
src/btree_nodes/plugins.xml Normal file
View File

@ -0,0 +1,5 @@
<library path="btree_nodes">
<class type="Factory" base_class_type="BT::BehaviorTreeFactory">
<description>Ye.</description>
</class>
</library>

16
src/btree_nodes/src/Factory.cpp Normal file
View File

@ -0,0 +1,16 @@
//
// Created by bastian on 18.08.22.
//
#include "Factory.h"
BT::ActorNodeFactory::ActorNodeFactory() {
auto YES = [](BT::TreeNode &parent_node) -> BT::NodeStatus {
return BT::NodeStatus::SUCCESS;
};
registerSimpleCondition("YES!", YES);
}
PLUGINLIB_EXPORT_CLASS(BT::ActorNodeFactory, BT::BehaviorTreeFactory)

18
src/btree_nodes/src/Factory.h Normal file
View File

@ -0,0 +1,18 @@
//
// Created by bastian on 18.08.22.
//
#ifndef BUILD_FACTORY_H
#define BUILD_FACTORY_H
#include <behaviortree_cpp_v3/bt_factory.h>
#include <pluginlib/class_list_macros.hpp>
namespace BT{
class ActorNodeFactory : public BT::BehaviorTreeFactory {
public:
ActorNodeFactory();
};
}
#endif //BUILD_FACTORY_H

1
src/btree_nodes/src/Tree.cpp
View File

@ -17,6 +17,7 @@
#include <thread> #include <thread>
int main(int argc, char **argv) { int main(int argc, char **argv) {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options; rclcpp::NodeOptions node_options;

314
src/gaz_simulation/world/gaz_ign.sdf Normal file
View File

@ -0,0 +1,314 @@
<?xml version="1.0" ?>
<sdf version='1.7'>
<world name='default'>
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-sensors-system"
name="ignition::gazebo::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name='walls'>
<static>1</static>
<link name='walls_1'>
<self_collide>0</self_collide>
<pose>3 8 1.2 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>10.1 .1 2.4</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>10.1 .1 2.4</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
<link name='walls_2'>
<self_collide>0</self_collide>
<pose>3 -2 1.2 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>10.1 .1 2.4</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>10.1 .1 2.4</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
<link name='walls_3'>
<self_collide>0</self_collide>
<pose>8 3 1.2 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>.1 9.9 2.4</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>.1 9.9 2.4</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
<link name='walls_4'>
<self_collide>0</self_collide>
<pose>-2 3 1.2 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>.1 9.9 2.4</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>.1 9.9 2.4</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name="lidar_1">
<static>1</static>
<link name="lidar_1_link">
<pose>1 -1.85 1 0 0 1.5707963267949</pose>
<sensor name='laser_1' type='gpu_lidar'>
<topic>laser_1</topic>
<update_rate>10</update_rate>
<lidar>
<scan>
<horizontal>
<samples>270</samples>
<resolution>1</resolution>
<min_angle>-2.3561944901923</min_angle>
<max_angle>2.3561944901923</max_angle>
</horizontal>
<vertical>
<samples>1</samples>
<resolution>1</resolution>
<min_angle>0</min_angle>
<max_angle>0</max_angle>
</vertical>
</scan>
<range>
<min>0.05</min>
<max>10.0</max>
<resolution>0.05</resolution>
</range>
</lidar>
<alwaysOn>1</alwaysOn>
<visualize>true</visualize>
</sensor>
</link>
</model>
<model name="lidar_2">
<static>1</static>
<link name="lidar_2_link">
<pose>-1.85 1 1 0 0 0</pose>
<sensor name='laser_2' type='gpu_lidar'>
<topic>laser_2</topic>
<update_rate>10</update_rate>
<lidar>
<scan>
<horizontal>
<samples>270</samples>
<resolution>1</resolution>
<min_angle>-2.3561944901923</min_angle>
<max_angle>2.3561944901923</max_angle>
</horizontal>
<vertical>
<samples>1</samples>
<resolution>1</resolution>
<min_angle>0</min_angle>
<max_angle>0</max_angle>
</vertical>
</scan>
<range>
<min>0.05</min>
<max>10.0</max>
<resolution>0.05</resolution>
</range>
</lidar>
<alwaysOn>1</alwaysOn>
<visualize>true</visualize>
</sensor>
</link>
</model>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<collide_bitmask>65535</collide_bitmask>
<ode/>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<actor name="actor1">
<pose>0 1 1.25 0 0 0</pose>
<skin>
<filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
<scale>1.0</scale>
</skin>
<animation name="walk">
<filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
</animation>
<!--<plugin name="actor1_plugin" filename="/home/bastian/dev_ws/build/gazebo_ros_actor/libRosActorPlugin.so">
<target>2 2 1</target>
<target_weight>1.15</target_weight>
<obstacle_weight>1.8</obstacle_weight>
<animation_factor>5.1</animation_factor>
<ignore_obstacles>
<model>ground_plane</model>
</ignore_obstacles>
</plugin>-->
</actor>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<state world_name='default'>
<sim_time>21 655000000</sim_time>
<real_time>21 863005214</real_time>
<wall_time>1644846646 70909467</wall_time>
<iterations>21655</iterations>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>5 -5 2 0 0.275643 2.35619</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>

15
src/ign_actor_plugin/CMakeLists.txt Normal file
View File

@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.24)
project(ActorPlugin)
set(IGN_PLUGIN_VER 0)
find_package(ignition-cmake2 REQUIRED)
find_package(ignition-gazebo5 REQUIRED)
ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register)
set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
# Add sources for each plugin to be registered.
add_library(SampleSystem src/ActorSystem.cpp)
set_property(TARGET SampleSystem PROPERTY CXX_STANDARD 17)
target_link_libraries(SampleSystem
ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
ignition-gazebo5::ignition-gazebo5
)

18
src/ign_actor_plugin/src/ActorSystem.cpp Normal file
View File

@ -0,0 +1,18 @@
//
// Created by bastian on 31.08.22.
//
#include "ActorSystem.h"
IGNITION_ADD_PLUGIN(
ActorSystem,
ignition::gazebo::System,
ActorSystem::ISystemPreUpdate)
ActorSystem::ActorSystem() = default;
ActorSystem::~ActorSystem() = default;
void ActorSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) {
}

26
src/ign_actor_plugin/src/ActorSystem.h Normal file
View File

@ -0,0 +1,26 @@
//
// Created by bastian on 31.08.22.
//
#ifndef BUILD_ACTORSYSTEM_H
#define BUILD_ACTORSYSTEM_H
#include <ignition/gazebo/System.hh>
#include <ignition/plugin/Register.hh>
class ActorSystem:
public ignition::gazebo::System,
public ignition::gazebo::ISystemPreUpdate {
public:
ActorSystem();
public:
~ActorSystem() override;
public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
ignition::gazebo::EntityComponentManager &_ecm) override;
};
#endif //BUILD_ACTORSYSTEM_H