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

Created a nonfunctional node for robot movement

Browse Source
This commit is contained in:
Bastian Hofmann 2022-03-27 19:26:14 +02:00
parent 2d2bc89a6b
commit e8ecafc194
12 changed files with 269 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/btree_nodes/CMakeLists.txt
View File

@ -8,18 +8,41 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(behaviortree_cpp_v3 REQUIRED)
set(DEPENDENCIES
ament_cmake
rclcpp
#geometry_msgs
std_msgs
behaviortree_cpp_v3
rclcpp_action
tf2_geometry_msgs
moveit_core
moveit_ros_planning_interface
tf2_geometry_msgs
moveit_ros_planning
)
foreach(dep IN LISTS DEPENDENCIES)
find_package(${dep} REQUIRED)
endforeach()
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(tree src/Tree.cpp src/Extensions.cpp src/nodes/WeightedRandomNode.cpp src/nodes/AmICalled.cpp src/nodes/GenerateTarget2D.cpp src/nodes/MoveActorToTarget.cpp src/helpers/MinimalSubscriber.cpp src/nodes/RobotMove.cpp)
ament_target_dependencies(tree rclcpp geometry_msgs std_msgs behaviortree_cpp_v3)
set(CPP_FILES
src/Tree.cpp
src/Extensions.cpp
src/nodes/WeightedRandomNode.cpp
src/nodes/AmICalled.cpp
src/nodes/GenerateTarget2D.cpp
src/nodes/MoveActorToTarget.cpp
src/helpers/MinimalSubscriber.cpp
src/nodes/RobotMove.cpp
)
add_executable(tree ${CPP_FILES})
ament_target_dependencies(tree ${DEPENDENCIES})
#add_executable(talker src/publisher_member_function.cpp)
#ament_target_dependencies(talker geometry_msgs rclcpp)

0
src/btree_nodes/tree.xml → src/btree_nodes/actorTree.xml
View File

10
src/btree_nodes/nodes.xml
View File

@ -30,8 +30,14 @@
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
<Condition ID="InTest">
<Condition ID="InAreaTest">
<input_port name="bounds">Bounds to check in</input_port>
<input_port name="position">Position of object</input_port>
<output_port name="result">Result of check if position is within bounds</output_port>
</Condition>
<Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port>
</Action>
</TreeNodesModel>
</root>

42
src/btree_nodes/robotTree.xml Normal file
View File

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<Action ID="RobotMove" target="0.2|0.2|1.1"/>
<!--<Action ID="RobotMove" target="-0.2|-0.2|1.1"/>-->
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="AmICalled">
<input_port name="called">called status</input_port>
</Action>
<Action ID="GenerateSafeTarget">
<output_port name="target">Generated pose in safe area</output_port>
</Action>
<Action ID="GenerateUnsafeTarget">
<output_port name="target">Generated pose in unsafe area</output_port>
</Action>
<Action ID="GenerateWarningTarget">
<output_port name="target">Generated pose in warning area</output_port>
</Action>
<Condition ID="InAreaTest">
<input_port name="bounds">Bounds to check in</input_port>
<input_port name="position">Position of object</input_port>
<output_port name="result">Result of check if position is within bounds</output_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="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>

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

@ -3,10 +3,11 @@
//
#include "Area.h"
#include <geometry_msgs/msg/pose.hpp>
namespace BT {
template<>
inline Position2D convertFromString(StringView str) {
Position2D convertFromString(StringView str) {
printf("Converting string: \"%s\"\n", str.data());
auto split = splitString(str, ';');
@ -21,7 +22,7 @@ namespace BT {
}
template<>
inline Area convertFromString(StringView str) {
Area convertFromString(StringView str) {
printf("Converting string: \"%s\"\n", str.data());
auto parts = splitString(str, '|');
@ -37,4 +38,28 @@ namespace BT {
output.triangles = array;
return output;
}
template<>
geometry_msgs::msg::Pose_<std::allocator<void> > convertFromString(StringView str) {
std::cout << "[ generating pose from string ]" << std::endl;
auto parts = splitString(str, '|');
if (parts.size() != 3) {
throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>|<y>|<z>'.");
}
geometry_msgs::msg::Pose 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.y= convertFromString<double>(parts[2]);
std::cout << "[ generated pose from string ]" << std::endl;
return pose;
}
}

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

@ -6,6 +6,7 @@
#include "nodes/AmICalled.h"
#include "nodes/GenerateTarget2D.h"
#include "nodes/MoveActorToTarget.h"
#include "nodes/RobotMove.h"
#include "helpers/MinimalSubscriber.h"
using namespace BT;
@ -36,6 +37,7 @@ int main(int argc, char **argv) {
rclcpp::init(argc, argv);
BehaviorTreeFactory factory;
factory.registerBuilder<GenerateTarget2D>("GenerateSafeTarget", builderGenerateSafeTarget);
factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateWarningTarget);
factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateUnsafeTarget);
@ -45,40 +47,50 @@ int main(int argc, char **argv) {
factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
factory.registerNodeType<RobotMove>("RobotMove");
std::cout << "Creating tree." << std::endl;
Tree tree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/tree.xml");
//Tree actorTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/actorTree.xml");
Tree robotTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/robotTree.xml");
/*
std::mutex mutex;
std::cout << "Starting subscriber." << std::endl;
geometry_msgs::msg::Pose init;
tree.rootBlackboard()->set<geometry_msgs::msg::Pose>("current",init);
tree.rootBlackboard()->set<geometry_msgs::msg::Pose>("target",init);
tree.rootBlackboard()->set<bool>("called",false);
actorTree.rootBlackboard()->set<geometry_msgs::msg::Pose>("current",init);
actorTree.rootBlackboard()->set<geometry_msgs::msg::Pose>("target",init);
actorTree.rootBlackboard()->set<bool>("called",false);
MinimalSubscriber<geometry_msgs::msg::Pose>::createAsThread("tree_get_currentPose", "currentPose",
[&tree, &mutex](geometry_msgs::msg::Pose pose) {
[&actorTree, &mutex](geometry_msgs::msg::Pose pose) {
mutex.lock();
tree.rootBlackboard()->set("current", pose);
actorTree.rootBlackboard()->set("current", pose);
mutex.unlock();
}).detach();
MinimalSubscriber<std_msgs::msg::Bool>::createAsThread("tree_get_called", "called",
[&tree, &mutex](std_msgs::msg::Bool called) {
[&actorTree, &mutex](std_msgs::msg::Bool called) {
mutex.lock();
tree.rootBlackboard()->set("called", (bool) called.data);
actorTree.rootBlackboard()->set("called", (bool) called.data);
mutex.unlock();
}).detach();
}).detach();*/
std::cout << "Looping." << std::endl;
/*std::thread actor([&actorTree, &mutex](){
while (rclcpp::ok()) {
mutex.lock();
tree.tickRoot();
actorTree.tickRoot();
mutex.unlock();
}
//});*/
while (rclcpp::ok()) {
robotTree.tickRoot();
}
std::cout << "End." << std::endl;

78
src/btree_nodes/src/nodes/RobotMove.cpp
View File

@ -3,3 +3,81 @@
//
#include "RobotMove.h"
BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &config)
: StatefulActionNode(name, config) {
std::cout << "[ constructing RobotMove ]" << std::endl;
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
char buffer[60];
sprintf(buffer,"RobotMove_%ld",random());
moveGroupNode = rclcpp::Node::make_shared(buffer, node_options);
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(moveGroupNode);
std::thread([&executor]() { executor.spin(); }).detach();
planningGroup="iisy";
moveGroup = std::make_shared<moveit::planning_interface::MoveGroupInterface>(moveGroupNode, planningGroup);
std::cout << "[ constructing RobotMove end ]" << std::endl;
}
BT::NodeStatus BT::RobotMove::onStart() {
std::cout << "[ start RobotMove ]" << std::endl;
geometry_msgs::msg::Pose pose;
if(!getInput<geometry_msgs::msg::Pose>("target",pose)){
std::cout << "[ no target position given ]" << std::endl;
return NodeStatus::FAILURE;
}
std::cout << "[ what the shit ]" << std::endl;
moveGroup->setPoseTarget(pose);
std::cout << "[ planning ]" << std::endl;
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (moveGroup->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
std::cout << "[ planned ]" << std::endl;
if(success) {
std::cout << "[ stopping async move ]" << std::endl;
moveGroup->stop();
std::cout << "[ stopped async move ]" << std::endl;
asyncThread.join();
asyncFinished = false;
asyncThread=std::thread([&my_plan, this](){
std::cout << "[ starting async move ]" << std::endl;
auto result = moveGroup->execute(my_plan);
std::cout << "[ finished async move ]" << std::endl;
if(result==moveit_msgs::msg::MoveItErrorCodes::SUCCESS){
asyncResult = NodeStatus::SUCCESS;
}else{
asyncResult = NodeStatus::FAILURE;
}
asyncFinished = true;
});
return NodeStatus::RUNNING;
}else{
return NodeStatus::FAILURE;
}
}
BT::NodeStatus BT::RobotMove::onRunning() {
if(asyncFinished){
return asyncResult;
}
return NodeStatus::RUNNING;
}
void BT::RobotMove::onHalted() {
moveGroup->stop();
asyncThread.join();
}
BT::PortsList BT::RobotMove::providedPorts() {
// Optionally, a port can have a human readable description
const char *description = "Generates a random position in an area defined by a triangle strip";
return {InputPort<geometry_msgs::msg::Pose>("target", description)};
}

24
src/btree_nodes/src/nodes/RobotMove.h
View File

@ -7,15 +7,31 @@
#include <behaviortree_cpp_v3/action_node.h>
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
namespace BT {
class RobotMove : public StatefulActionNode {
public:
RobotMove(const std::string &name, const BT::NodeConfiguration &config)
: StatefulActionNode(name, config) {
private:
std::shared_ptr<rclcpp::Node> moveGroupNode;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> moveGroup;
std::string planningGroup;
std::thread asyncThread;
NodeStatus asyncResult = NodeStatus::FAILURE;
bool asyncFinished = false;
}
public:
RobotMove(const std::string &name, const BT::NodeConfiguration &config);
NodeStatus onStart() override;
NodeStatus onRunning() override;
void onHalted() override;
static PortsList providedPorts();
};
}

1
src/btree_nodes/src/nodes/WeightedRandomNode.cpp
View File

@ -57,6 +57,7 @@ NodeStatus WeightedRandomNode::tick() {
select_next = false;
}
std::cout<< "Running child " << selected_child_index << std::endl;
TreeNode *current_child_node = children_nodes_[selected_child_index];
const NodeStatus child_status = current_child_node->executeTick();

44
src/btree_robot/CMakeLists.txt
View File

@ -11,42 +11,42 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
rclcpp
rclcpp_action
tf2_geometry_msgs
tf2_ros
# tf2_ros
moveit_core
rviz_visual_tools
moveit_visual_tools
# rviz_visual_tools
# moveit_visual_tools
moveit_ros_planning_interface
interactive_markers
# interactive_markers
tf2_geometry_msgs
moveit_ros_planning
pluginlib
Eigen3
Boost
control_msgs
moveit_servo
# pluginlib
# Eigen3
# Boost
# control_msgs
# moveit_servo
)
find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED system filesystem date_time thread)
#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(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(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(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

4
src/gazebo_ros_actor/CMakeLists.txt
View File

@ -8,7 +8,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rospy REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(gazebo REQUIRED)
link_directories(${GAZEBO_LIBRARY_DIRS})
@ -20,7 +20,7 @@ include_directories(
add_library(RosActorPlugin SHARED src/RosActorPlugin.cc)
target_link_libraries(RosActorPlugin ${GAZEBO_LIBRARIES})
ament_target_dependencies(RosActorPlugin geometry_msgs rclcpp)
ament_target_dependencies(RosActorPlugin tf2_geometry_msgs rclcpp)
install(
TARGETS

6
src/iisy_config/launch/lbr_move_group.launch.py
View File

@ -141,9 +141,9 @@ def launch_setup(context, *args, **kwargs):
# MoveGroupInterface demo executable
move_group_demo = Node(
name="btree_robot",
package="btree_robot",
executable="robot_test",
name="btree_nodes",
package="btree_nodes",
executable="tree",
output="screen",
parameters=[
robot_description,