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

added thread based moveit node, works once for now

Browse Source
This commit is contained in:
Bastian Hofmann 2022-03-28 18:07:30 +02:00
parent e8ecafc194
commit 406cba6989
13 changed files with 169 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/btree_nodes/CMakeLists.txt
View File

@ -39,6 +39,7 @@ set(CPP_FILES
src/nodes/MoveActorToTarget.cpp
src/helpers/MinimalSubscriber.cpp
src/nodes/RobotMove.cpp
src/nodes/MoveConnection.cpp
)
add_executable(tree ${CPP_FILES})

2
src/btree_nodes/robotTree.xml
View File

@ -4,7 +4,7 @@
<BehaviorTree ID="BehaviorTree">
<Sequence>
<Action ID="RobotMove" target="0.2|0.2|1.1"/>
<!--<Action ID="RobotMove" target="-0.2|-0.2|1.1"/>-->
<Action ID="RobotMove" target="-0.2|-0.2|1.1"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->

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

@ -40,7 +40,7 @@ namespace BT {
}
template<>
geometry_msgs::msg::Pose_<std::allocator<void> > convertFromString(StringView str) {
std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
std::cout << "[ generating pose from string ]" << std::endl;
@ -49,14 +49,14 @@ namespace BT {
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]);
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]);
std::cout << "[ generated pose from string ]" << std::endl;

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

@ -8,11 +8,25 @@
#include "nodes/MoveActorToTarget.h"
#include "nodes/RobotMove.h"
#include "helpers/MinimalSubscriber.h"
#include <chrono>
#include <thread>
using namespace BT;
//using namespace BT;
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto mainNode = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(mainNode);
std::thread([&executor]() { executor.spin(); }).detach();
BehaviorTreeFactory factory;
/*
std::cout << "Registering nodes." << std::endl;
NodeBuilder builderGenerateSafeTarget = [](const std::string &name, const NodeConfiguration &config) {
const Area area = {std::vector<Position2D>({{1, 1},
@ -34,10 +48,6 @@ int main(int argc, char **argv) {
return std::make_unique<GenerateTarget2D>(name, config, area);
};
rclcpp::init(argc, argv);
BehaviorTreeFactory factory;
factory.registerBuilder<GenerateTarget2D>("GenerateSafeTarget", builderGenerateSafeTarget);
factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateWarningTarget);
factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateUnsafeTarget);
@ -46,8 +56,15 @@ int main(int argc, char **argv) {
factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
*/
factory.registerNodeType<RobotMove>("RobotMove");
auto connection = std::make_shared<MoveConnection>(mainNode,"iisy");
NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
return std::make_unique<RobotMove>(name, config, &connection);
};
factory.registerBuilder<RobotMove>("RobotMove",builderIisyMove);
std::cout << "Creating tree." << std::endl;
@ -78,6 +95,8 @@ int main(int argc, char **argv) {
mutex.unlock();
}).detach();*/
std::cout << "Looping." << std::endl;
/*std::thread actor([&actorTree, &mutex](){
@ -90,6 +109,7 @@ int main(int argc, char **argv) {
while (rclcpp::ok()) {
robotTree.tickRoot();
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
std::cout << "End." << std::endl;

1
src/btree_nodes/src/helpers/MinimalSubscriber.h
View File

@ -39,7 +39,6 @@ std::thread MinimalSubscriber<T>::createAsThread(const std::string &node_name, c
auto subscriber = std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback);
return std::thread([subscriber]() {
rclcpp::spin(subscriber);
rclcpp::shutdown();
});
}

2
src/btree_nodes/src/nodes/GenerateTarget2D.cpp
View File

@ -36,7 +36,7 @@ Position2D BT::GenerateTarget2D::getRandomPosInTriangle(unsigned long id) {
return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)};
}
BT::GenerateTarget2D::GenerateTarget2D(const std::string &name, const BT::NodeConfiguration &config, const Area area) :
BT::GenerateTarget2D::GenerateTarget2D(const std::string &name, const BT::NodeConfiguration &config, const Area& area) :
SyncActionNode(name, config) {
this->area = area;
unsigned long triangleCount = area.triangles.size() - 2;

2
src/btree_nodes/src/nodes/GenerateTarget2D.h
View File

@ -28,7 +28,7 @@ private:
Position2D getRandomPosInTriangle(unsigned long id);
public:
GenerateTarget2D(const std::string &name, const NodeConfiguration &config, const Area area);
GenerateTarget2D(const std::string &name, const NodeConfiguration &config, const Area& area);
static PortsList providedPorts();

5
src/btree_nodes/src/nodes/MoveConnection.cpp Normal file
View File

@ -0,0 +1,5 @@
//
// Created by bastian on 28.03.22.
//
#include "MoveConnection.h"

58
src/btree_nodes/src/nodes/MoveConnection.h Normal file
View File

@ -0,0 +1,58 @@
//
// Created by bastian on 28.03.22.
//
#ifndef BUILD_MOVECONNECTION_H
#define BUILD_MOVECONNECTION_H
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
class MoveConnection {
private:
moveit::planning_interface::MoveGroupInterface moveGroup;
std::thread running;
std::mutex lock;
public:
MoveConnection(const std::shared_ptr<rclcpp::Node>& node, const std::string& planningGroup): moveGroup(node,planningGroup) {
}
void planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,const std::function<void(bool)>& callback){
lock.lock();
printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
std::cout << "planning" << std::endl;
moveGroup.setStartStateToCurrentState();
moveGroup.setPoseTarget(*pose);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if(!success){
callback(false);
}
running = std::thread([this,callback,my_plan](){
std::cout << "executing" << std::endl;
moveGroup.stop();
bool success = moveGroup.execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
std::cout << "callback" << std::endl;
callback(success);
std::cout << "end" << std::endl;
lock.unlock();
});
}
void stop(){
std::cout << "stopping move" << std::endl;
moveGroup.stop();
if(running.joinable()){
running.join();
}
std::cout << "move stopped." << std::endl;
}
};
#endif //BUILD_MOVECONNECTION_H

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

@ -4,76 +4,42 @@
#include "RobotMove.h"
BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &config)
BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &config,
std::shared_ptr<MoveConnection> *connection)
: 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;
this->connection = connection;
}
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;
std::shared_ptr<geometry_msgs::msg::Pose> pose;
if(!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target",pose)){
std::cout << "no target available." << 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;
}
printf("X:%f Y:%f Z:%f\n",pose->position.x,pose->position.y,pose->position.z);
printf("W:%f X:%f Y:%f Z:%f\n",pose->orientation.w,pose->orientation.x,pose->orientation.y,pose->orientation.z);
asyncResult = NodeStatus::RUNNING;
connection->get()->planAndExecute(pose,[this](bool finished){
if(finished){
asyncResult = NodeStatus::SUCCESS;
} else {
asyncResult = NodeStatus::FAILURE;
}
});
return NodeStatus::RUNNING;
}
BT::NodeStatus BT::RobotMove::onRunning() {
auto result = asyncResult;
if(asyncResult!=NodeStatus::RUNNING){
asyncResult=NodeStatus::IDLE;
}
return result;
}
void BT::RobotMove::onHalted() {
moveGroup->stop();
asyncThread.join();
connection->get()->stop();
asyncResult=NodeStatus::IDLE;
}
BT::PortsList BT::RobotMove::providedPorts() {

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

@ -10,20 +10,18 @@
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "MoveConnection.h"
namespace BT {
class RobotMove : public StatefulActionNode {
private:
std::shared_ptr<rclcpp::Node> moveGroupNode;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> moveGroup;
std::string planningGroup;
std::thread asyncThread;
std::shared_ptr<MoveConnection> *connection;
NodeStatus asyncResult = NodeStatus::FAILURE;
bool asyncFinished = false;
public:
RobotMove(const std::string &name, const BT::NodeConfiguration &config);
RobotMove(const std::string &name, const BT::NodeConfiguration &config,
std::shared_ptr<MoveConnection> *connection);
NodeStatus onStart() override;

6
src/btree_robot/CMakeLists.txt
View File

@ -58,11 +58,11 @@ find_package(behaviortree_cpp_v3 REQUIRED)
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(robot_test src/robot_test.cpp)
ament_target_dependencies(robot_test ${THIS_PACKAGE_INCLUDE_DEPENDS} std_msgs behaviortree_cpp_v3)
add_executable(tree src/robot_test.cpp)
ament_target_dependencies(tree ${THIS_PACKAGE_INCLUDE_DEPENDS} std_msgs behaviortree_cpp_v3)
install(TARGETS
robot_test
tree
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)

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

@ -40,78 +40,49 @@
// 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");
//static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");
int main(int argc, char** argv)
{
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);
int main(int argc, char **argv) {
int i;
printf("%d\n", argc);
for (i = 0; i < argc - 1; i++) {
printf("%s\n", argv[i]);
}
// We spin up a SingleThreadedExecutor for the current state monitor to get information
// about the robot's state.
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread([&executor]() { executor.spin(); }).detach();
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);
// BEGIN_TUTORIAL
//
// Setup
// ^^^^^
//
// MoveIt operates on sets of joints called "planning groups" and stores them in an object called
// the ``JointModelGroup``. Throughout MoveIt, the terms "planning group" and "joint model group"
// are used interchangeably.
static const std::string PLANNING_GROUP = "iisy";
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread([&executor]() { executor.spin(); }).detach();
// The
// :moveit_codedir:`MoveGroupInterface<moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h>`
// class can be easily set up using just the name of the planning group you would like to control and plan for.
moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
std::string PLANNING_GROUP = "iisy";
moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
// We will use the
// :moveit_codedir:`PlanningSceneInterface<moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h>`
// class to add and remove collision objects in our "virtual world" scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
geometry_msgs::msg::Pose target_pose1;
target_pose1.orientation.w = 0;
target_pose1.orientation.y = 1;
// Raw pointers are frequently used to refer to the planning group for improved performance.
const moveit::core::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
target_pose1.position.x = 0.2;
target_pose1.position.y = -0.2;
target_pose1.position.z = 1.2;
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
// Start the demo
// ^^^^^^^^^^^^^^^^^^^^^^^^^
// visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
// 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.
// .. _move_group_interface-planning-to-pose-goal:
//
// Planning to a Pose goal
// ^^^^^^^^^^^^^^^^^^^^^^^
// We can plan a motion for this group to a desired pose for the
// end-effector.
geometry_msgs::msg::Pose target_pose1;
target_pose1.orientation.w = 0;
target_pose1.orientation.y = 1;
//RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
target_pose1.position.x = 0.2;
target_pose1.position.y = -0.2;
target_pose1.position.z = 1.2;
move_group.setPoseTarget(target_pose1);
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();
// 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.
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
move_group.asyncExecute(my_plan.trajectory_);
move_group.execute(my_plan);
rclcpp::shutdown();
return 0;
rclcpp::shutdown();
return 0;
}