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

Compare commits

base: yenon:67256759ad5101a6a9820f4f066c15bab4e6a8e3
Branches Tags
yenon:master
..
compare: yenon:6bda1077c29f62bea5f8cd8b5f0a8c1a2724d267
Branches Tags
yenon:master

No commits in common. "67256759ad5101a6a9820f4f066c15bab4e6a8e3" and "6bda1077c29f62bea5f8cd8b5f0a8c1a2724d267" have entirely different histories.
67256759ad ... 6bda1077c2

54 changed files with 271 additions and 648 deletions
Show Stats Expand all files Collapse all files

20
src/btree/trees/actorTreeCoex.xml
View File

@ -1,20 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="actorTree">
<BehaviorTree ID="actorTree">
<Control ID="WeightedRandom" weights="95,5">
<Sequence>
<Control ID="WeightedRandom" weights="95,5">
<Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
</Control>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
<Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
<Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
</Sequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
</Sequence>
</Control>
</BehaviorTree>
</root>

73
src/btree/trees/actorTreeCoop.xml
View File

@ -1,73 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="actorTree">
<!-- ////////// -->
<BehaviorTree ID="actorTree">
<Fallback>
<ReactiveSequence>
<Inverter>
<Condition ID="IsCalled"/>
</Inverter>
<Sequence>
<Control ID="WeightedRandom" weights="100,5,2">
<Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
</Control>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
</Sequence>
</ReactiveSequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
<Action ID="SetCalledTo" state="false"/>
</Sequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="ActorAnimation">
<input_port name="animation">Name of animation to play</input_port>
</Action>
<Action ID="ActorMovement">
<input_port name="animation">Name of animation to play</input_port>
<input_port name="target">Pose to move to</input_port>
</Action>
<Condition ID="IsCalled"/>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action>
<Action ID="GenerateXYPose">
<input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in area</output_port>
</Action>
<Condition ID="InAreaTest">
<input_port name="area">Bounds to check in</input_port>
<input_port name="pose">Position of object</input_port>
</Condition>
<Action ID="MoveActorToTarget">
<input_port name="current">Current actor position</input_port>
<input_port name="target">Target to move actor to</input_port>
</Action>
<Action ID="PositionToPose">
<input_port name="offset">offset as a Point</input_port>
<input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
<input_port name="output">generated new pose</input_port>
<input_port name="position">initial position as Position2D</input_port>
</Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port>
</Action>
<Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port>
</Action>
<Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
</TreeNodesModel>
<!-- ////////// -->
</root>

25
src/btree/trees/robotTreeCoex.xml
View File

@ -1,25 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="robotTree">
<BehaviorTree ID="robotTree">
<ReactiveSequence>
<Inverter>
<Condition ID="InAreaTest" area="{unsafeArea}" pose="{actorPos}"/>
</Inverter>
<IfThenElse>
<Condition ID="InAreaTest" area="{warningArea}" pose="{actorPos}"/>
<Action ID="SetRobotVelocity" velocity="0.1"/>
<Action ID="SetRobotVelocity" velocity="1"/>
</IfThenElse>
<Fallback>
<Control ID="InterruptableSequence">
<Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/>
<Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/>
</Control>
</Fallback>
</ReactiveSequence>
</BehaviorTree>
</root>

77
src/btree/trees/robotTreeCoop.xml
View File

@ -1,77 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="robotTree">
<!-- ////////// -->
<BehaviorTree ID="robotTree">
<ReactiveSequence>
<Inverter>
<Condition ID="InAreaTest" area="{unsafeArea}" pose="{actorPos}"/>
</Inverter>
<IfThenElse>
<Condition ID="InAreaTest" area="{warningArea}" pose="{actorPos}"/>
<Action ID="SetRobotVelocity" velocity="0.1"/>
<Action ID="SetRobotVelocity" velocity="1"/>
</IfThenElse>
<Control ID="InterruptableSequence">
<Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/>
<Control ID="WeightedRandom" weights="90,10">
<Sequence>
<Action ID="OffsetPose" input="-0.68,0,1.215" offset="0,0,0" orientation="0,-0.707,0,0.707" output="{target}"/>
<Action ID="RobotMove" target="{target}"/>
</Sequence>
<Sequence>
<Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/>
<Action ID="SetCalledTo" state="true"/>
</Sequence>
</Control>
</Control>
</ReactiveSequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="ActorAnimation">
<input_port name="animation">Name of animation to play</input_port>
</Action>
<Action ID="ActorMovement">
<input_port name="animation">Name of animation to play</input_port>
<input_port name="target">Pose to move to</input_port>
</Action>
<Condition ID="IsCalled"/>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action>
<Action ID="GenerateXYPose">
<input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in area</output_port>
</Action>
<Condition ID="InAreaTest">
<input_port name="area">Bounds to check in</input_port>
<input_port name="pose">Position of object</input_port>
</Condition>
<Action ID="OffsetPose">
<input_port name="input">initial position as Pose</input_port>
<input_port name="offset">offset as a Point</input_port>
<input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
<output_port name="output">generated new pose</output_port>
</Action>
<Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port>
</Action>
<Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port>
</Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
<Control ID="InterruptableSequence">
</Control>
</TreeNodesModel>
<!-- ////////// -->
</root>

30
src/btree/CMakeLists.txt → src/btree_nodes/CMakeLists.txt
View File

@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
project(btree) project(btree_nodes)
add_compile_options(-Wall -Wextra -Wpedantic) add_compile_options(-Wall -Wextra -Wpedantic)
set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD_REQUIRED ON)
@ -36,8 +36,10 @@ 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/GenerateXYPose.cpp src/nodes/GenerateXYPose.cpp
src/nodes/RobotMove.cpp src/nodes/RobotMove.cpp
src/nodes/MoveConnection.cpp src/nodes/MoveConnection.cpp
@ -50,15 +52,30 @@ set(CPP_FILES
src/nodes/ActorMovement.cpp src/nodes/ActorMovement.cpp
) )
add_library(tree_plugins_base src/Factory.cpp)
set_property(TARGET tree_plugins_base PROPERTY CXX_STANDARD 17)
add_executable(tree ${CPP_FILES}) add_executable(tree ${CPP_FILES})
set_property(TARGET tree PROPERTY CXX_STANDARD 17) set_property(TARGET tree PROPERTY CXX_STANDARD 17)
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)
#ament_target_dependencies(talker geometry_msgs rclcpp)
#add_executable(listener src/subscriber_member_function.cpp)
#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})
install(DIRECTORY trees DESTINATION share/${PROJECT_NAME})
if (BUILD_TESTING) if (BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
@ -71,4 +88,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.xml → src/btree_nodes/nodes.xml
View File

@ -70,7 +70,7 @@
</Action> </Action>
<Action ID="RandomFailure"> <Action ID="RandomFailure">
<input_port name="failure_chance">Chance to fail from 0 to 1</input_port> <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
</Action> </Action>
</TreeNodesModel> </TreeNodesModel>
</root> </root>

2
src/btree/package.xml → src/btree_nodes/package.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3"> <package format="3">
<name>btree</name> <name>btree_nodes</name>
<version>0.0.0</version> <version>0.0.0</version>
<description>TODO: Package description</description> <description>TODO: Package description</description>
<maintainer email="bastian@todo.todo">bastian</maintainer> <maintainer email="bastian@todo.todo">bastian</maintainer>

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>

0
src/btree/src/Area.h → src/btree_nodes/src/Area.h
View File

19
src/btree/src/Extensions.cpp → src/btree_nodes/src/Extensions.cpp
View File

@ -38,22 +38,15 @@ namespace BT {
template<> template<>
std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) { std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
auto parts = splitString(str, ','); auto parts = splitString(str, ',');
if (!(parts.size() == 3 || parts.size() == 7)) { if (parts.size() != 3) {
throw RuntimeError("Incorrect number of arguments, expected 3 or 7 in format '<x>,<y>,<z>[,<qw>,<qx>,<qy>,<qz>]'."); throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
} }
auto pose = std::make_shared<geometry_msgs::msg::Pose>(); auto pose = std::make_shared<geometry_msgs::msg::Pose>();
if(parts.size() == 7){ pose->orientation.w = 1;
pose->orientation.w = convertFromString<double>(parts[3]); pose->orientation.x = 0;
pose->orientation.x = convertFromString<double>(parts[4]); pose->orientation.y = 0;
pose->orientation.y = convertFromString<double>(parts[5]); pose->orientation.z = 0;
pose->orientation.z = convertFromString<double>(parts[6]);
}else{
pose->orientation.w = 1;
pose->orientation.x = 0;
pose->orientation.y = 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]);

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

2
src/btree/src/Tree.cpp → src/btree_nodes/src/Tree.cpp
View File

@ -8,6 +8,7 @@
#include <rclcpp/utilities.hpp> #include <rclcpp/utilities.hpp>
#include <std_msgs/msg/bool.hpp> #include <std_msgs/msg/bool.hpp>
#include "nodes/WeightedRandomNode.h" #include "nodes/WeightedRandomNode.h"
#include "nodes/AmICalled.h"
#include "nodes/GenerateXYPose.h" #include "nodes/GenerateXYPose.h"
#include "nodes/RobotMove.h" #include "nodes/RobotMove.h"
#include "nodes/OffsetPose.h" #include "nodes/OffsetPose.h"
@ -39,6 +40,7 @@ int main(int argc, char **argv) {
std::cout << "Registering nodes." << std::endl; std::cout << "Registering nodes." << std::endl;
factory.registerNodeType<GenerateXYPose>("GenerateXYPose"); factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
factory.registerNodeType<AmICalled>("AmICalled");
factory.registerNodeType<WeightedRandomNode>("WeightedRandom"); factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
factory.registerNodeType<OffsetPose>("OffsetPose"); factory.registerNodeType<OffsetPose>("OffsetPose");

18
src/btree/src/nodes/ActorAnimation.cpp → src/btree_nodes/src/nodes/ActorAnimation.cpp
View File

@ -12,8 +12,7 @@ ActorAnimation::ActorAnimation(const std::string &name, const BT::NodeConfigurat
BT::PortsList ActorAnimation::providedPorts() { BT::PortsList ActorAnimation::providedPorts() {
return { return {
BT::InputPort<std::string>("animation_name"), BT::InputPort<std::string>("animation"),
BT::InputPort<float>("animation_speed"),
}; };
} }
@ -22,16 +21,15 @@ BT::NodeStatus ActorAnimation::onStart() {
ros_actor_action_server_msgs::action::Animation::Goal goal; ros_actor_action_server_msgs::action::Animation::Goal goal;
if (!getInput<std::string>("animation_name", goal.animation_name)) { auto res = getInput<std::string>("animation", goal.animation_name);
if (!res) {
std::cerr << "[ no animation name specified ]" << std::endl; std::cerr << "[ no animation name specified ]" << std::endl;
std::cout << res.error() << std::endl;
return BT::NodeStatus::FAILURE; return BT::NodeStatus::FAILURE;
} }
if (!getInput<float>("animation_speed", goal.animation_speed)) {
goal.animation_speed=1.0;
}
goal.animation_speed=1.0;
auto send_goal_options = Client<Animation>::SendGoalOptions(); auto send_goal_options = Client<Animation>::SendGoalOptions();
send_goal_options.result_callback = [=](const ClientGoalHandle<Animation>::WrappedResult & parameter) { send_goal_options.result_callback = [=](const ClientGoalHandle<Animation>::WrappedResult & parameter) {
@ -46,16 +44,12 @@ BT::NodeStatus ActorAnimation::onStart() {
client->async_send_goal(goal,send_goal_options); client->async_send_goal(goal,send_goal_options);
result = BT::NodeStatus::RUNNING;
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} }
BT::NodeStatus ActorAnimation::onRunning() { BT::NodeStatus ActorAnimation::onRunning() {
mutex.lock(); mutex.lock();
auto status = result; auto status = result;
if(result != BT::NodeStatus::RUNNING){
result = BT::NodeStatus::IDLE;
}
mutex.unlock(); mutex.unlock();
return status; return status;
} }

0
src/btree/src/nodes/ActorAnimation.h → src/btree_nodes/src/nodes/ActorAnimation.h
View File

29
src/btree/src/nodes/ActorMovement.cpp → src/btree_nodes/src/nodes/ActorMovement.cpp
View File

@ -16,8 +16,7 @@ ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguratio
BT::PortsList ActorMovement::providedPorts() { BT::PortsList ActorMovement::providedPorts() {
return { return {
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target"), BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target"),
BT::InputPort<std::string>("animation_name"), BT::InputPort<std::string>("animation"),
BT::InputPort<float>("animation_distance"),
}; };
} }
@ -26,24 +25,20 @@ BT::NodeStatus ActorMovement::onStart() {
ros_actor_action_server_msgs::action::Movement::Goal goal; ros_actor_action_server_msgs::action::Movement::Goal goal;
{ std::shared_ptr<geometry_msgs::msg::Pose> target;
std::shared_ptr<geometry_msgs::msg::Pose> target; auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target)) {
std::cout << "[ no target given ]" << std::endl; if (!res) {
return BT::NodeStatus::FAILURE; std::cout << "[ no target available ]" << std::endl;
} std::cout << res.error() << std::endl;
goal.target = *target;
}
if(!getInput<float>("animation_distance", goal.animation_distance)){
goal.animation_distance = 1.0f;
}
if(!getInput<std::string>("animation_name", goal.animation_name)){
std::cout << "[ no animation_name given ]" << std::endl;
return BT::NodeStatus::FAILURE; return BT::NodeStatus::FAILURE;
} }
goal.animation_distance=1.5;
goal.animation_name="walk";
goal.animation_speed=1.0;
goal.target = *target;
auto send_goal_options = Client<Movement>::SendGoalOptions(); auto send_goal_options = Client<Movement>::SendGoalOptions();
send_goal_options.feedback_callback = [&](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, std::shared_ptr<const Movement::Feedback> feedback) { send_goal_options.feedback_callback = [&](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, std::shared_ptr<const Movement::Feedback> feedback) {
positionCallback(feedback); positionCallback(feedback);

0
src/btree/src/nodes/ActorMovement.h → src/btree_nodes/src/nodes/ActorMovement.h
View File

23
src/btree_nodes/src/nodes/AmICalled.cpp Normal file
View File

@ -0,0 +1,23 @@
//
// Created by bastian on 26.03.22.
//
#include "AmICalled.h"
BT::AmICalled::AmICalled(const std::string &name, const BT::NodeConfiguration &config) :
ConditionNode(name, config) {}
BT::PortsList BT::AmICalled::providedPorts() {
return {InputPort<bool>("called")};
}
BT::NodeStatus BT::AmICalled::tick() {
bool called;
if (!getInput("called", called)) {
return NodeStatus::FAILURE;
}
if (called) {
return NodeStatus::SUCCESS;
}
return NodeStatus::FAILURE;
}

22
src/btree_nodes/src/nodes/AmICalled.h Normal file
View File

@ -0,0 +1,22 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_AMICALLED_H
#define BUILD_AMICALLED_H
#include <behaviortree_cpp_v3/condition_node.h>
namespace BT {
class AmICalled : public ConditionNode {
public:
AmICalled(const std::string &name, const NodeConfiguration &config);
static PortsList providedPorts();
NodeStatus tick() override;
};
}
#endif //BUILD_AMICALLED_H

5
src/btree/src/nodes/GenerateXYPose.cpp → src/btree_nodes/src/nodes/GenerateXYPose.cpp
View File

@ -77,11 +77,6 @@ BT::NodeStatus BT::GenerateXYPose::tick() {
auto randomPose = std::make_shared<geometry_msgs::msg::Pose>(); auto randomPose = std::make_shared<geometry_msgs::msg::Pose>();
randomPose->position.x = pos.x; randomPose->position.x = pos.x;
randomPose->position.y = pos.y; randomPose->position.y = pos.y;
randomPose->position.z = 0;
randomPose->orientation.w = 0;
randomPose->orientation.x = 0;
randomPose->orientation.y = 0;
randomPose->orientation.z = 0;
printf("Generated Target: %f %f\n", pos.x, pos.y); printf("Generated Target: %f %f\n", pos.x, pos.y);
setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", randomPose); setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("pose", randomPose);

0
src/btree/src/nodes/GenerateXYPose.h → src/btree_nodes/src/nodes/GenerateXYPose.h
View File

0
src/btree/src/nodes/InAreaTest.cpp → src/btree_nodes/src/nodes/InAreaTest.cpp
View File

0
src/btree/src/nodes/InAreaTest.h → src/btree_nodes/src/nodes/InAreaTest.h
View File

0
src/btree/src/nodes/InterruptableSequence.cpp → src/btree_nodes/src/nodes/InterruptableSequence.cpp
View File

0
src/btree/src/nodes/InterruptableSequence.h → src/btree_nodes/src/nodes/InterruptableSequence.h
View File

0
src/btree/src/nodes/MoveConnection.cpp → src/btree_nodes/src/nodes/MoveConnection.cpp
View File

0
src/btree/src/nodes/MoveConnection.h → src/btree_nodes/src/nodes/MoveConnection.h
View File

27
src/btree/src/nodes/OffsetPose.cpp → src/btree_nodes/src/nodes/OffsetPose.cpp
View File

@ -3,16 +3,14 @@
// //
#include "OffsetPose.h" #include "OffsetPose.h"
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <memory>
BT::PortsList BT::OffsetPose::providedPorts() { BT::PortsList BT::OffsetPose::providedPorts() {
return { return {
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Pose"), InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Position2D"),
InputPort<std::shared_ptr<geometry_msgs::msg::Point>>("offset", "offset as a Point"), InputPort<std::shared_ptr<geometry_msgs::msg::Point>>("offset", "offset as a Point"),
InputPort<std::shared_ptr<geometry_msgs::msg::Quaternion>>("orientation", "rotation of resulting pose as Quaternion"), InputPort<std::shared_ptr<geometry_msgs::msg::Quaternion>>("orientation", "rotation of resulting pose as Quaternion"),
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose") InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose")
}; };
} }
@ -20,30 +18,29 @@ BT::OffsetPose::OffsetPose(const std::string &name,
const BT::NodeConfiguration &config) : SyncActionNode(name, config) {} const BT::NodeConfiguration &config) : SyncActionNode(name, config) {}
BT::NodeStatus BT::OffsetPose::tick() { BT::NodeStatus BT::OffsetPose::tick() {
std::shared_ptr<geometry_msgs::msg::Pose> pose; std::shared_ptr<geometry_msgs::msg::Pose> pose;
if (!getInput("input", pose)) { if (!getInput("input", pose)) {
return NodeStatus::FAILURE; return NodeStatus::FAILURE;
} }
auto resultPose = std::make_shared<geometry_msgs::msg::Pose>(*pose);
std::shared_ptr<geometry_msgs::msg::Quaternion> quaternion; std::shared_ptr<geometry_msgs::msg::Quaternion> quaternion;
if(getInput("orientation", quaternion)) { if(getInput("orientation", quaternion)) {
resultPose->orientation.w = quaternion->w; pose->orientation.w = quaternion->w;
resultPose->orientation.x = quaternion->x; pose->orientation.x = quaternion->x;
resultPose->orientation.y = quaternion->y; pose->orientation.y = quaternion->y;
resultPose->orientation.z = quaternion->z; pose->orientation.z = quaternion->z;
} }
std::shared_ptr<geometry_msgs::msg::Point> offset; std::shared_ptr<geometry_msgs::msg::Point> offset;
if(getInput("offset", offset)) { if(getInput("offset", offset)) {
resultPose->position.x += offset->x; pose->position.x += offset->x;
resultPose->position.y += offset->y; pose->position.y += offset->y;
resultPose->position.z += offset->z; pose->position.z += offset->z;
} }
setOutput("output", resultPose); setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("output", pose);
return BT::NodeStatus::SUCCESS; return BT::NodeStatus::SUCCESS;
} }

0
src/btree/src/nodes/OffsetPose.h → src/btree_nodes/src/nodes/OffsetPose.h
View File

4
src/btree/src/nodes/RandomFailure.cpp → src/btree_nodes/src/nodes/RandomFailure.cpp
View File

@ -6,7 +6,7 @@
BT::NodeStatus BT::RandomFailure::tick() { BT::NodeStatus BT::RandomFailure::tick() {
double failureChance; double failureChance;
if (!getInput<double>("failure_chance", failureChance)) { if (!getInput<double>("failureChance", failureChance)) {
std::cout << "could not get failure chance." << std::endl; std::cout << "could not get failure chance." << std::endl;
return NodeStatus::FAILURE; return NodeStatus::FAILURE;
} }
@ -19,7 +19,7 @@ BT::NodeStatus BT::RandomFailure::tick() {
} }
BT::PortsList BT::RandomFailure::providedPorts() { BT::PortsList BT::RandomFailure::providedPorts() {
return {InputPort<double>("failure_chance", "Chance to fail from 0 to 1")}; return {InputPort<double>("failureChance", "Chance to fail from 0 to 1")};
} }
BT::RandomFailure::RandomFailure( BT::RandomFailure::RandomFailure(

0
src/btree/src/nodes/RandomFailure.h → src/btree_nodes/src/nodes/RandomFailure.h
View File

4
src/btree/src/nodes/RobotMove.cpp → src/btree_nodes/src/nodes/RobotMove.cpp
View File

@ -13,7 +13,7 @@ BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &c
BT::NodeStatus BT::RobotMove::onStart() { BT::NodeStatus BT::RobotMove::onStart() {
std::cout << "Starting RobotMove" << std::endl; std::cout << "Starting RobotMove" << std::endl;
std::shared_ptr<geometry_msgs::msg::Pose> pose; std::shared_ptr<geometry_msgs::msg::Pose> pose;
if(!getInput("target",pose)){ if(!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target",pose)){
std::cout << "no target available." << std::endl; std::cout << "no target available." << std::endl;
return NodeStatus::FAILURE; return NodeStatus::FAILURE;
} }
@ -22,10 +22,8 @@ BT::NodeStatus BT::RobotMove::onStart() {
asyncResult = NodeStatus::RUNNING; asyncResult = NodeStatus::RUNNING;
connection->get()->planAndExecute(pose,[this](bool finished){ connection->get()->planAndExecute(pose,[this](bool finished){
if(finished){ if(finished){
printf("Finished move.");
asyncResult = NodeStatus::SUCCESS; asyncResult = NodeStatus::SUCCESS;
} else { } else {
printf("Failed move.");
asyncResult = NodeStatus::FAILURE; asyncResult = NodeStatus::FAILURE;
} }
}); });

0
src/btree/src/nodes/RobotMove.h → src/btree_nodes/src/nodes/RobotMove.h
View File

0
src/btree/src/nodes/SetRobotVelocity.cpp → src/btree_nodes/src/nodes/SetRobotVelocity.cpp
View File

0
src/btree/src/nodes/SetRobotVelocity.h → src/btree_nodes/src/nodes/SetRobotVelocity.h
View File

0
src/btree/src/nodes/WeightedRandomNode.cpp → src/btree_nodes/src/nodes/WeightedRandomNode.cpp
View File

0
src/btree/src/nodes/WeightedRandomNode.h → src/btree_nodes/src/nodes/WeightedRandomNode.h
View File

15
src/controller_test/CMakeLists.txt → src/btree_trees/CMakeLists.txt
View File

@ -1,28 +1,17 @@
cmake_minimum_required(VERSION 3.8) cmake_minimum_required(VERSION 3.8)
project(controller_test) project(btree_trees)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic) add_compile_options(-Wall -Wextra -Wpedantic)
endif() endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# find dependencies # find dependencies
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(control_msgs REQUIRED)
# uncomment the following section in order to fill in # uncomment the following section in order to fill in
# further dependencies manually. # further dependencies manually.
# find_package(<dependency> REQUIRED) # find_package(<dependency> REQUIRED)
add_executable(move src/test.cpp) install(DIRECTORY trees DESTINATION share/${PROJECT_NAME})
set_property(TARGET move PROPERTY CXX_STANDARD 17)
ament_target_dependencies(move rclcpp moveit_ros_planning_interface control_msgs)
install(TARGETS
move
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)

13
src/btree_trees/package.xml Normal file
View File

@ -0,0 +1,13 @@
<package format="3">
<name>btree_trees</name>
<version>0.244.1</version>
<description>Trees for the simulation</description>
<license>Apache 2.0</license>
<maintainer email="bastian.hofmann@xitaso.com">Bastian Hofmann</maintainer>
<buildtool_depend>ament_cmake</buildtool_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

8
src/btree/trees/actorTreeColab.xml → src/btree_trees/trees/actorTree.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<root main_tree_to_execute="actorTree"> <root main_tree_to_execute="BehaviorTree">
<!-- ////////// --> <!-- ////////// -->
<BehaviorTree ID="actorTree"> <BehaviorTree ID="BehaviorTree">
<Fallback> <Fallback>
<ReactiveSequence> <ReactiveSequence>
<Inverter> <Inverter>
@ -14,12 +14,12 @@
<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}"/>
</Control> </Control>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/> <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
</Sequence> </Sequence>
</ReactiveSequence> </ReactiveSequence>
<Sequence> <Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/> <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/> <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
<Action ID="SetCalledTo" state="false"/> <Action ID="SetCalledTo" state="false"/>
</Sequence> </Sequence>
</Fallback> </Fallback>

8
src/btree/trees/robotTreeColab.xml → src/btree_trees/trees/robotTree.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<root main_tree_to_execute="robotTree"> <root main_tree_to_execute="BehaviorTree">
<!-- ////////// --> <!-- ////////// -->
<BehaviorTree ID="robotTree"> <BehaviorTree ID="BehaviorTree">
<ReactiveSequence> <ReactiveSequence>
<Inverter> <Inverter>
<Condition ID="InAreaTest" area="{unsafeArea}" pose="{actorPos}"/> <Condition ID="InAreaTest" area="{unsafeArea}" pose="{actorPos}"/>
@ -16,11 +16,11 @@
<Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/> <Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/> <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/> <Action ID="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failure_chance="0.15"/> <Action ID="RandomFailure" failureChance="0.15"/>
<Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/> <Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
<Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/> <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/> <Action ID="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failure_chance="0.05"/> <Action ID="RandomFailure" failureChance="0.05"/>
</Control> </Control>
<Action ID="SetCalledTo" state="true"/> <Action ID="SetCalledTo" state="true"/>
</Fallback> </Fallback>

23
src/controller_test/package.xml
View File

@ -1,23 +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>controller_test</name>
<version>0.1.0</version>
<description>
Test for Moveit on IISY
</description>
<author email="bastian.hofmann@xitaso.com">bastian</author>
<maintainer email="bastian.hofmann@xitaso.com">bastian</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<depend>moveit_ros_planning_interface</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

203
src/controller_test/src/test.cpp
View File

@ -1,203 +0,0 @@
#include <control_msgs/action/detail/follow_joint_trajectory__struct.hpp>
#include <memory>
#include <rclcpp_action/client_goal_handle.hpp>
#include <rclcpp_action/types.hpp>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <control_msgs/action/follow_joint_trajectory.hpp>
std::shared_ptr<rclcpp::Node> node;
void common_goal_response(
const rclcpp_action::ClientGoalHandle
<control_msgs::action::FollowJointTrajectory>::SharedPtr& future)
{
RCLCPP_DEBUG(
node->get_logger(), "common_goal_response time: %f",
rclcpp::Clock().now().seconds());
auto goal_handle = future.get();
if (!goal_handle) {
printf("Goal rejected\n");
} else {
printf("Goal accepted\n");
}
}
void common_result_response(
const rclcpp_action::ClientGoalHandle
<control_msgs::action::FollowJointTrajectory>::WrappedResult & result)
{
printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
printf("SUCCEEDED result code\n");
break;
case rclcpp_action::ResultCode::ABORTED:
printf("Goal was aborted\n");
return;
case rclcpp_action::ResultCode::CANCELED:
printf("Goal was canceled\n");
return;
default:
printf("Unknown result code\n");
return;
}
}
void common_feedback(
const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr&,
const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback>& feedback)
{
std::cout << "feedback->desired.positions :";
for (auto & x : feedback->desired.positions) {
std::cout << x << "\t";
}
std::cout << std::endl;
std::cout << "feedback->desired.velocities :";
for (auto & x : feedback->desired.velocities) {
std::cout << x << "\t";
}
std::cout << std::endl;
}
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
node = std::make_shared<rclcpp::Node>("trajectory_test_node");
std::cout << "node created" << std::endl;
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
node->get_node_base_interface(),
node->get_node_graph_interface(),
node->get_node_logging_interface(),
node->get_node_waitables_interface(),
"/arm_controller/follow_joint_trajectory");
bool response =
action_client->wait_for_action_server(std::chrono::seconds(10));
if (!response) {
throw std::runtime_error("could not get action server");
}
std::cout << "Created action server" << std::endl;
std::vector<std::string> joint_names = {
"base_rot",
"base_link1_joint",
"link1_link2_joint",
"link2_rot",
"link2_link3_joint",
"link3_rot"
};
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
trajectory_msgs::msg::JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap
point.positions.resize(joint_names.size());
point.positions[0] = 0.0;
point.positions[1] = 0.0;
point.positions[2] = 0.0;
point.positions[3] = 0.0;
point.positions[4] = 0.0;
point.positions[5] = 0.0;
trajectory_msgs::msg::JointTrajectoryPoint point2;
point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
point2.positions.resize(joint_names.size());
point2.positions[0] = -1.0;
point2.positions[1] = 0.0;
point2.positions[2] = 0.0;
point2.positions[3] = 0.0;
point2.positions[4] = 0.0;
point2.positions[5] = 0.0;
trajectory_msgs::msg::JointTrajectoryPoint point3;
point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
point3.positions.resize(joint_names.size());
point3.positions[0] = 1.0;
point3.positions[1] = 0.0;
point3.positions[2] = 0.0;
point3.positions[3] = 0.0;
point3.positions[4] = 0.0;
point3.positions[5] = 0.0;
trajectory_msgs::msg::JointTrajectoryPoint point4;
point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
point4.positions.resize(joint_names.size());
point4.positions[0] = 0.0;
point4.positions[1] = 0.0;
point4.positions[2] = M_PI/2;
point4.positions[3] = 0.0;
point4.positions[4] = 0.0;
point4.positions[5] = 0.0;
points.push_back(point);
points.push_back(point2);
points.push_back(point3);
points.push_back(point4);
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
opt.goal_response_callback = [](std::shared_ptr<rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>> goal){
if(goal->get_status()==rclcpp_action::GoalStatus::STATUS_ACCEPTED){
RCLCPP_DEBUG(
node->get_logger(), "common_goal_response accepted: %f",
rclcpp::Clock().now().seconds());
}else{
RCLCPP_DEBUG(
node->get_logger(), "common_goal_response rejected: %f",
rclcpp::Clock().now().seconds());
}
};
//opt.goal_response_callback = [](auto && PH1) { return common_goal_response(std::forward<decltype(PH1)>(PH1)); };
opt.result_callback = [](auto && PH1) { return common_result_response(std::forward<decltype(PH1)>(PH1)); };
opt.feedback_callback = [](auto && PH1, auto && PH2) { return common_feedback(std::forward<decltype(PH1)>(PH1), std::forward<decltype(PH2)>(PH2)); };
control_msgs::action::FollowJointTrajectory_Goal goal_msg;
goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
goal_msg.trajectory.joint_names = joint_names;
goal_msg.trajectory.points = points;
auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
return 1;
}
RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr&
goal_handle = goal_handle_future.get();
if (!goal_handle) {
RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
return 1;
}
RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");
// Wait for the server to be done with the goal
auto result_future = action_client->async_get_result(goal_handle);
RCLCPP_INFO(node->get_logger(), "Waiting for result");
if (rclcpp::spin_until_future_complete(node, result_future) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
return 1;
}
std::cout << "async_send_goal" << std::endl;
rclcpp::shutdown();
return 0;
}

BIN
src/ign_actor_plugin/animation/new_actor_rig_3.blend
View File

Binary file not shown.

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

@ -124,12 +124,11 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity); auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
auto newTime = animationTimeComponent->Data() + _info.dt; auto newTime = animationTimeComponent->Data() + _info.dt;
auto newTimeSeconds = std::chrono::duration<double>(newTime).count(); auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
auto progress = newTimeSeconds / (duration/animation_speed); feedback.progress = newTimeSeconds / duration;
feedback.progress = progress;
sendFeedback(); sendFeedback();
if (progress >= 1.0) { if (newTimeSeconds >= duration) {
igndbg << "Animation " << animation_name << " finished." << std::endl; igndbg << "Animation " << animation_name << " finished." << std::endl;
nextState = IDLE; nextState = IDLE;
feedback.progress = 0.0f; feedback.progress = 0.0f;
@ -167,7 +166,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
if(movementDetails.targetDiff.Pos().Length() >= 0.001){ if(movementDetails.targetDiff.Pos().Length() >= 0.001){
movementDetails.targetDiff.Rot() = ignition::math::Quaterniond::EulerToQuaternion(0.0, 0.0, atan2(movementDetails.targetDiff.Pos().Y(), movementDetails.targetDiff.Pos().X())); movementDetails.targetDiff.Rot() = ignition::math::Quaterniond::EulerToQuaternion(0.0, 0.0, atan2(movementDetails.targetDiff.Pos().Y(), movementDetails.targetDiff.Pos().X()));
movementDetails.rotateToTargetDuration = Angle(actorPose.Rot(),movementDetails.targetDiff.Rot()) / turnSpeed; movementDetails.rotateToTargetDuration = Angle(actorPose.Rot(),movementDetails.targetDiff.Rot()) / turnSpeed;
movementDetails.moveDuration = movementDetails.targetDiff.Pos().Length() / animation_speed * duration; movementDetails.moveDuration = movementDetails.targetDiff.Pos().Length() / animation_distance * duration;
}else{ }else{
movementDetails.targetDiff.Rot() = movementDetails.start.Rot(); movementDetails.targetDiff.Rot() = movementDetails.start.Rot();
movementDetails.rotateToTargetDuration = 0.0; movementDetails.rotateToTargetDuration = 0.0;
@ -191,7 +190,6 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
if (movementDetails.state == movementDetails.ROTATE_TO_TARGET){ if (movementDetails.state == movementDetails.ROTATE_TO_TARGET){
if(movementDetails.stepTime<=movementDetails.rotateToTargetDuration){ if(movementDetails.stepTime<=movementDetails.rotateToTargetDuration){
newTime = newTime.zero();
actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToTargetDuration,movementDetails.start.Rot(),movementDetails.targetDiff.Rot(),true); actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToTargetDuration,movementDetails.start.Rot(),movementDetails.targetDiff.Rot(),true);
}else{ }else{
actorPose.Rot() = movementDetails.targetDiff.Rot(); actorPose.Rot() = movementDetails.targetDiff.Rot();
@ -313,7 +311,8 @@ void ActorSystem::messageQueueInterface(const char name[256]) {
threadMutex.lock(); threadMutex.lock();
strcpy(animation_name, receivedAction.animationName); strcpy(animation_name, receivedAction.animationName);
animation_speed = receivedAction.animationSpeed; // animation_speed = receivedAction.animationSpeed;
animation_distance = receivedAction.animationDistance;
movementDetails.target.Pos().Set(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z); movementDetails.target.Pos().Set(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
movementDetails.target.Rot().Set(receivedAction.target.orientation.w, receivedAction.target.orientation.x, receivedAction.target.orientation.y, receivedAction.target.orientation.z); movementDetails.target.Rot().Set(receivedAction.target.orientation.w, receivedAction.target.orientation.x, receivedAction.target.orientation.y, receivedAction.target.orientation.z);
nextState = receivedAction.state; nextState = receivedAction.state;

2
src/ign_actor_plugin/src/ActorSystem.hpp
View File

@ -59,7 +59,7 @@ class ActorSystem : public System,
private: private:
ignition::math::Pose3d startPose = ignition::math::Pose3d::Zero; ignition::math::Pose3d startPose = ignition::math::Pose3d::Zero;
ignition::math::Pose3d target_pose; ignition::math::Pose3d target_pose;
double animation_speed; double animation_distance;
char animation_name[256]; char animation_name[256];
ActorPluginState nextState,currentState,lastState; ActorPluginState nextState,currentState,lastState;
double duration{}; double duration{};

1
src/ign_world/CMakeLists.txt
View File

@ -12,7 +12,6 @@ find_package(ament_cmake REQUIRED)
# find_package(<dependency> REQUIRED) # find_package(<dependency> REQUIRED)
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
install(DIRECTORY model DESTINATION share/${PROJECT_NAME})
install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
install(DIRECTORY world DESTINATION share/${PROJECT_NAME}) install(DIRECTORY world DESTINATION share/${PROJECT_NAME})

105
src/ign_world/launch/gazebo_controller_launch.py
View File

@ -31,70 +31,12 @@ def generate_launch_description():
], ],
output='screen' output='screen'
) )
btree = Node(
package='btree',
executable='tree',
output='both',
parameters=[
{
"trees": [
get_package_share_directory('btree')+'/trees/actorTreeCoex.xml',
get_package_share_directory('btree')+'/trees/robotTreeCoex.xml'
],
"areas": [
"safeArea",
"1, 3.5 |" +
"1, 7 |" +
"3, 3.5 |" +
"7, 7 |" +
"3, -1 |" +
"7, -1",
"warningArea",
"-1, 2.5 |" +
"-1, 3.5 |" +
"2, 2.5 |" +
"3, 3.5 |" +
"2, -1 |" +
"3, -1",
"unsafeArea",
"-1, 1.5 |" +
"-1, 2.5 |" +
"1, 1.5 |" +
"2, 2.5 |" +
"1, -1 |" +
"2, -1",
"negativeYTable",
"0.3, -0.25 |" +
"-0.3, -0.25 |" +
"0, -0.4",
"positiveYTable",
"0.3, 0.25 |" +
"-0.3, 0.25 |" +
"0, 0.4"
]
},
],
emulate_tty=True,
)
return LaunchDescription([ return LaunchDescription([
IncludeLaunchDescription( IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')), PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
launch_arguments={'gz_args': '-v 4 -r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(), launch_arguments={'gz_args': '-v 4 -r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
), ),
Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', 'office',
'-allow_renaming', 'true',
'-string', xacro.process(os.path.join(get_package_share_directory('ign_world'), 'world', 'conveyor.sdf')),
#'-file', 'https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Gazebo',
],
output='screen'
),
Node( Node(
package='ros_gz_bridge', package='ros_gz_bridge',
@ -109,10 +51,52 @@ def generate_launch_description():
package='ros_actor_action_server', package='ros_actor_action_server',
executable='ros_actor_action_server', executable='ros_actor_action_server',
output='both', output='both',
emulate_tty=True,
), ),
btree, Node(
package='btree_nodes',
executable='tree',
output='both',
parameters=[
{
"trees": [
get_package_share_directory('btree_trees')+'/trees/actorTree.xml',
get_package_share_directory('btree_trees')+'/trees/robotTree.xml'
],
"areas": [
"safeArea",
"1, 3.5 |"+
"1, 7 |"+
"3, 3.5 |"+
"7, 7 |"+
"3, -1 |"+
"7, -1",
"warningArea",
"-1, 2.5 |"+
"-1, 3.5 |"+
"2, 2.5 |"+
"3, 3.5 |"+
"2, -1 |"+
"3, -1",
"unsafeArea",
"-1, 1.5 |"+
"-1, 2.5 |"+
"1, 1.5 |"+
"2, 2.5 |"+
"1, -1 |"+
"2, -1",
"negativeYTable",
"0.3, -0.25 |"+
"-0.3, -0.25 |"+
"0, -0.4",
"positiveYTable",
"0.3, 0.25 |"+
"-0.3, 0.25 |"+
"0, 0.4"
]
},
]
),
IncludeLaunchDescription( IncludeLaunchDescription(
PythonLaunchDescriptionSource( PythonLaunchDescriptionSource(
@ -132,7 +116,6 @@ def generate_launch_description():
on_exit=[load_joint_state_broadcaster], on_exit=[load_joint_state_broadcaster],
) )
), ),
RegisterEventHandler( RegisterEventHandler(
event_handler=OnProcessExit( event_handler=OnProcessExit(
target_action=load_joint_state_broadcaster, target_action=load_joint_state_broadcaster,

BIN
src/ign_world/model/conveyor.stl
View File

Binary file not shown.

19
src/ign_world/world/conveyor.sdf
View File

@ -1,19 +0,0 @@
<sdf version="1.6">
<model name="office">
<static>true</static>
<link name="office">
<visual name="visual">
<geometry>
<mesh>
<uri>file://$(find ign_world)/model/conveyor.stl</uri>
</mesh>
</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>
</sdf>

64
src/iisy_config/config/iisy.urdf
View File

@ -5,29 +5,29 @@
<link name="table"> <link name="table">
<collision> <collision>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/table.stl"/> <mesh filename="$(find iisy_config)/stl/table.stl"/>
</geometry> </geometry>
</collision> </collision>
<visual> <visual>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/table.stl"/> <mesh filename="$(find iisy_config)/stl/table.stl"/>
</geometry> </geometry>
</visual> </visual>
</link> </link>
<link name="base_bottom"> <link name="base_bottom">
<collision> <collision>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/base_bottom_coll.stl"/> <mesh filename="$(find iisy_config)/stl/base_bottom_coll.stl"/>
</geometry> </geometry>
</collision> </collision>
<visual> <visual>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/base_bottom_low.stl"/> <mesh filename="$(find iisy_config)/stl/base_bottom_low.stl"/>
</geometry> </geometry>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="-0.043517 0.000000 0.054914"/> <origin rpy="0 0 0" xyz="-0.043517 0.000000 0.054914"/>
<mass value="6"/> <mass value="0.1"/>
<inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/> <inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
</inertial> </inertial>
</link> </link>
@ -40,29 +40,29 @@
</collision> </collision>
<visual> <visual>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/base_top_low.stl"/> <mesh filename="$(find iisy_config)/stl/base_top_low.stl"/>
</geometry> </geometry>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 -0.005 0.08"/> <origin rpy="0 0 0" xyz="0 -0.005 0.08"/>
<mass value="2"/> <mass value="0.1"/>
<inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/> <inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
</inertial> </inertial>
</link> </link>
<link name="link1_full"> <link name="link1_full">
<collision> <collision>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/link_1_full_coll.stl"/> <mesh filename="$(find iisy_config)/stl/link_1_full_coll.stl"/>
</geometry> </geometry>
</collision> </collision>
<visual> <visual>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/link_1_full_low.stl"/> <mesh filename="$(find iisy_config)/stl/link_1_full_low.stl"/>
</geometry> </geometry>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 -0.050000 0.150000"/> <origin rpy="0 0 0" xyz="0 -0.050000 0.150000"/>
<mass value="4"/> <mass value="0.1"/>
<inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/> <inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
</inertial> </inertial>
</link> </link>
@ -75,63 +75,63 @@
</collision> </collision>
<visual> <visual>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/link_2_bottom_low.stl"/> <mesh filename="$(find iisy_config)/stl/link_2_bottom_low.stl"/>
</geometry> </geometry>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0.07 0.025"/> <origin rpy="0 0 0" xyz="0 0.07 0.025"/>
<mass value="2"/> <mass value="0.1"/>
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/> <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
</inertial> </inertial>
</link> </link>
<link name="link2_top"> <link name="link2_top">
<collision> <collision>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/link_2_top_coll.stl"/> <mesh filename="$(find iisy_config)/stl/link_2_top_coll.stl"/>
</geometry> </geometry>
</collision> </collision>
<visual> <visual>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/link_2_top_low.stl"/> <mesh filename="$(find iisy_config)/stl/link_2_top_low.stl"/>
</geometry> </geometry>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0.037363 0.086674"/> <origin rpy="0 0 0" xyz="0 0.037363 0.086674"/>
<mass value="2"/> <mass value="0.1"/>
<inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/> <inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
</inertial> </inertial>
</link> </link>
<link name="link3_bottom"> <link name="link3_bottom">
<collision> <collision>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/link_3_bottom_coll.stl"/> <mesh filename="$(find iisy_config)/stl/link_3_bottom_coll.stl"/>
</geometry> </geometry>
</collision> </collision>
<visual> <visual>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/link_3_bottom_low.stl"/> <mesh filename="$(find iisy_config)/stl/link_3_bottom_low.stl"/>
</geometry> </geometry>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 -0.055000 0.021413"/> <origin rpy="0 0 0" xyz="0 -0.055000 0.021413"/>
<mass value="2"/> <mass value="0.1"/>
<inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/> <inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
</inertial> </inertial>
</link> </link>
<link name="link3_top"> <link name="link3_top">
<collision> <collision>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/link_3_top_coll.stl"/> <mesh filename="$(find iisy_config)/stl/link_3_top_coll.stl"/>
</geometry> </geometry>
</collision> </collision>
<visual> <visual>
<geometry> <geometry>
<mesh filename="file://$(find iisy_config)/stl/link_3_top_low.stl"/> <mesh filename="$(find iisy_config)/stl/link_3_top_low.stl"/>
</geometry> </geometry>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 -0.015462 0.040000"/> <origin rpy="0 0 0" xyz="0 -0.015462 0.040000"/>
<mass value="0.8"/> <mass value="0.1"/>
<inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/> <inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
</inertial> </inertial>
</link> </link>
@ -155,31 +155,31 @@
<parent link="base_bottom"/> <parent link="base_bottom"/>
<child link="base_top"/> <child link="base_top"/>
<axis xyz="0 0 1"/> <axis xyz="0 0 1"/>
<limit effort="300" velocity="3.49065850399"/> <limit effort="30" velocity="1.7453292519943"/>
<dynamics damping="30.0" friction="0.1"/> <dynamics damping="10.0" friction="0.1"/>
</joint> </joint>
<joint name="base_link1_joint" type="revolute"> <joint name="base_link1_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.080499 0.092997"/> <origin rpy="0 0 0" xyz="0 -0.080499 0.092997"/>
<parent link="base_top"/> <parent link="base_top"/>
<child link="link1_full"/> <child link="link1_full"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<limit effort="300" lower="-2.2689280275926" upper="2.4434609527921" velocity="3.49065850399"/> <limit effort="30" lower="-2.2689280275926" upper="2.4434609527921" velocity="1.7453292519943"/>
<dynamics damping="30.0" friction="0.1"/> <dynamics damping="10.0" friction="0.1"/>
</joint> </joint>
<joint name="link1_link2_joint" type="revolute"> <joint name="link1_link2_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.3"/> <origin rpy="0 0 0" xyz="0 0 0.3"/>
<parent link="link1_full"/> <parent link="link1_full"/>
<child link="link2_bottom"/> <child link="link2_bottom"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<limit effort="200" lower="-2.6179938779915" upper="2.6179938779915" velocity="3.49065850399"/> <limit effort="30" lower="-2.6179938779915" upper="2.6179938779915" velocity="1.7453292519943"/>
<dynamics damping="20.0" friction="0.1"/> <dynamics damping="10.0" friction="0.1"/>
</joint> </joint>
<joint name="link2_rot" type="continuous"> <joint name="link2_rot" type="continuous">
<origin rpy="0 0 0" xyz="0 0.080501 0.120502"/> <origin rpy="0 0 0" xyz="0 0.080501 0.120502"/>
<parent link="link2_bottom"/> <parent link="link2_bottom"/>
<child link="link2_top"/> <child link="link2_top"/>
<axis xyz="0 0 1"/> <axis xyz="0 0 1"/>
<limit effort="100" velocity="5.23598775598"/> <limit effort="30" velocity="1.7453292519943"/>
<dynamics damping="10.0" friction="0.1"/> <dynamics damping="10.0" friction="0.1"/>
</joint> </joint>
<joint name="link2_link3_joint" type="continuous"> <joint name="link2_link3_joint" type="continuous">
@ -187,16 +187,16 @@
<parent link="link2_top"/> <parent link="link2_top"/>
<child link="link3_bottom"/> <child link="link3_bottom"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<limit effort="50" velocity="5.23598775598"/> <limit effort="30" velocity="1.7453292519943"/>
<dynamics damping="5.0" friction="0.1"/> <dynamics damping="10.0" friction="0.1"/>
</joint> </joint>
<joint name="link3_rot" type="continuous"> <joint name="link3_rot" type="continuous">
<origin rpy="0 0 0" xyz="0 -0.055001 0.085501"/> <origin rpy="0 0 0" xyz="0 -0.055001 0.085501"/>
<parent link="link3_bottom"/> <parent link="link3_bottom"/>
<child link="link3_top"/> <child link="link3_top"/>
<axis xyz="0 0 1"/> <axis xyz="0 0 1"/>
<limit effort="50" velocity="6.98131700798"/> <limit effort="30" velocity="1.7453292519943"/>
<dynamics damping="5.0" friction="0.1"/> <dynamics damping="10.0" friction="0.1"/>
</joint> </joint>
<transmission name="trans_base_rot"> <transmission name="trans_base_rot">
<type>transmission_interface/SimpleTransmission</type> <type>transmission_interface/SimpleTransmission</type>

3
src/ros_actor_action_server/src/Server.cpp
View File

@ -122,7 +122,6 @@ int main(int argc, char **argv) {
if (currentFeedback.state == IDLE) { if (currentFeedback.state == IDLE) {
std::cout << "Accepting..." << std::endl; std::cout << "Accepting..." << std::endl;
currentAction.animationSpeed = animationGoal->animation_speed;
strcpy(currentAction.animationName, animationGoal->animation_name.data()); strcpy(currentAction.animationName, animationGoal->animation_name.data());
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
@ -164,7 +163,7 @@ int main(int argc, char **argv) {
std::cout << "Accepting..." << std::endl; std::cout << "Accepting..." << std::endl;
currentAction.target = movementGoal->target; currentAction.target = movementGoal->target;
currentAction.animationSpeed = movementGoal->animation_distance; currentAction.animationDistance = movementGoal->animation_distance;
strcpy(currentAction.animationName, movementGoal->animation_name.data()); strcpy(currentAction.animationName, movementGoal->animation_name.data());
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;

1
src/ros_actor_action_server_msgs/action/Movement.action
View File

@ -1,4 +1,5 @@
string animation_name string animation_name
float32 animation_speed
float32 animation_distance float32 animation_distance
geometry_msgs/Pose target geometry_msgs/Pose target
--- ---

20
src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp
View File

@ -1,21 +1,19 @@
#include <geometry_msgs/msg/detail/pose__struct.hpp> #include <geometry_msgs/msg/detail/pose__struct.hpp>
namespace ros_actor_message_queue_msgs{ namespace ros_actor_message_queue_msgs{
enum ActorPluginState{ enum ActorPluginState{
SETUP,IDLE,ANIMATION,MOVEMENT SETUP,IDLE,ANIMATION,MOVEMENT
}; };
struct FeedbackMessage{ struct FeedbackMessage{
ActorPluginState state; ActorPluginState state;
float progress; float progress;
geometry_msgs::msg::Pose current; geometry_msgs::msg::Pose current;
}; };
struct ActionMessage{ struct ActionMessage{
ActorPluginState state; ActorPluginState state;
geometry_msgs::msg::Pose target; geometry_msgs::msg::Pose target;
float animationSpeed = 1.0f; float animationSpeed = 1.0f,animationDistance = 1.0f;
char animationName[256]; char animationName[256];
}; };
} }