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

Tree working again, just robot simulation for now

Browse Source
This commit is contained in:
Bastian Hofmann 2023-02-28 09:25:50 +00:00
parent 49c8a25111
commit d9074b1103
8 changed files with 159 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/btree_nodes/CMakeLists.txt
View File

@ -22,6 +22,7 @@ set(DEPENDENCIES
tf2_geometry_msgs
moveit_ros_planning
pluginlib
ros_actor_action_server_msgs
)
foreach(dep IN LISTS DEPENDENCIES)
@ -48,6 +49,7 @@ set(CPP_FILES
src/nodes/InAreaTest.cpp
src/nodes/InterruptableSequence.cpp
src/nodes/SetRobotVelocity.cpp
src/nodes/ActorMovement.cpp
)
add_library(tree_plugins_base src/Factory.cpp)

1
src/btree_nodes/package.xml
View File

@ -10,6 +10,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<depend>ros_actor_action_server_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

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

@ -13,6 +13,7 @@
#include "nodes/InAreaTest.h"
#include "nodes/InterruptableSequence.h"
#include "nodes/SetRobotVelocity.h"
#include "nodes/ActorMovement.h"
#include <chrono>
#include <thread>
@ -23,7 +24,8 @@ int main(int argc, char **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);
auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
auto moveNode = rclcpp::Node::make_shared("tree_moveit_node", node_options);
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(mainNode);
@ -33,46 +35,46 @@ int main(int argc, char **argv) {
const std::shared_ptr<Area> safeArea = std::make_shared<Area>();
safeArea->triangles = {std::vector<Position2D>({
{-1, 3.5},
{-1, 7},
{3, 3.5},
{7, 7},
{3, -1},
{7, -1}
{ 1, 3.5 },
{ 1, 7 },
{ 3, 3.5 },
{ 7, 7 },
{ 3, -1 },
{ 7, -1 }
})};
const std::shared_ptr<Area> warningArea = std::make_shared<Area>();
warningArea->triangles = {std::vector<Position2D>({
{-1, 2.5},
{-1, 3.5},
{2, 2.5},
{3, 3.5},
{2, -1},
{3, -1}
{ -1, 2.5 },
{ -1, 3.5 },
{ 2, 2.5 },
{ 3, 3.5 },
{ 2, -1 },
{ 3, -1 }
})};
const std::shared_ptr<Area> unsafeArea = std::make_shared<Area>();
unsafeArea->triangles = std::vector<Position2D>({
{-1, 1.5},
{-1, 2.5},
{1, 1.5},
{2, 2.5},
{1, -1},
{2, -1}
{ -1, 1.5 },
{ -1, 2.5 },
{ 1, 1.5 },
{ 2, 2.5 },
{ 1, -1 },
{ 2, -1 }
});
const std::shared_ptr<Area> negativeYTable = std::make_shared<Area>();
negativeYTable->triangles = std::vector<Position2D>({
{0.3,-0.25},
{-0.3,-0.25},
{0,-0.4}
{ 0.3, -0.25 },
{ -0.3, -0.25 },
{ 0, -0.4 }
});
const std::shared_ptr<Area> positiveYTable = std::make_shared<Area>();
positiveYTable->triangles = std::vector<Position2D>({
{0.3,0.25},
{-0.3,0.25},
{0,0.4}
{ 0.3, 0.25 },
{ -0.3, 0.25 },
{ 0, 0.4 }
});
std::cout << "Registering nodes." << std::endl;
@ -80,6 +82,7 @@ int main(int argc, char **argv) {
factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
factory.registerNodeType<AmICalled>("AmICalled");
factory.registerNodeType<ActorMovement>("ActorMovement");
factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
factory.registerNodeType<OffsetPose>("OffsetPose");
@ -88,7 +91,7 @@ int main(int argc, char **argv) {
factory.registerNodeType<InterruptableSequence>("InterruptableSequence");
auto connection = std::make_shared<MoveConnection>(mainNode, "iisy");
auto connection = std::make_shared<MoveConnection>(moveNode, "arm");
NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
return std::make_unique<RobotMove>(name, config, &connection);
@ -119,11 +122,13 @@ int main(int argc, char **argv) {
std::cout << "Creating tree." << std::endl;
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");
Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml");
Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml");
auto trees = {actorTree.rootBlackboard(), robotTree.rootBlackboard()};
auto init = std::make_shared<geometry_msgs::msg::Pose>();
init->position.x = 6.0;
init->position.y = 6.0;
for (const auto &item : trees){
item->set("safeArea",safeArea);
@ -147,12 +152,12 @@ int main(int argc, char **argv) {
std::cout << "Looping." << std::endl;
std::thread actor([&actorTree]() {
/*std::thread actor([&actorTree]() {
while (rclcpp::ok()) {
actorTree.tickRoot();
std::this_thread::sleep_for(std::chrono::milliseconds(20));
}
});
});*/
while (rclcpp::ok()) {
robotTree.tickRoot();

64
src/btree_nodes/src/nodes/ActorMovement.cpp Normal file
View File

@ -0,0 +1,64 @@
//
// Created by bastian on 26.03.22.
//
#include "ActorMovement.h"
BT::ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
: StatefulActionNode(name, config) {}
BT::PortsList BT::ActorMovement::providedPorts() {
return {
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
};
}
BT::NodeStatus BT::ActorMovement::onStart() {
std::cout << "started moving" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
if (!res) {
std::cout << "[ no target available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
}
publisher->publish(*target);
return NodeStatus::RUNNING;
}
BT::NodeStatus BT::ActorMovement::onRunning() {
std::shared_ptr<geometry_msgs::msg::Pose> current;
auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("current", current);
if (!res) {
std::cout << "[ no current position available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
}
double dX = target->position.x - current->position.x;
double dY = target->position.y - current->position.y;
auto distance = sqrt(dX * dX + dY * dY);
if (distance < 0.3) {
return NodeStatus::SUCCESS;
} else {
return NodeStatus::RUNNING;
}
}
void BT::ActorMovement::onHalted() {
std::cout << "halted move" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
geometry_msgs::msg::Pose inf;
inf.position.x = HUGE_VAL;
inf.position.y = HUGE_VAL;
publisher->publish(inf);
}

30
src/btree_nodes/src/nodes/ActorMovement.h Normal file
View File

@ -0,0 +1,30 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_ACTORMOVEMENT_H
#define BUILD_ACTORMOVEMENT_H
#include <behaviortree_cpp_v3/action_node.h>
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/rclcpp.hpp>
namespace BT {
class ActorMovement : public StatefulActionNode {
public:
ActorMovement(const std::string &name, const NodeConfiguration &config);
static PortsList providedPorts();
std::shared_ptr<geometry_msgs::msg::Pose> target;
NodeStatus onStart() override;
NodeStatus onRunning() override;
void onHalted() override;
};
}
#endif //BUILD_ACTORMOVEMENT_H

25
src/btree_nodes/src/nodes/MoveConnection.cpp
View File

@ -6,32 +6,39 @@
void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
const std::function<void(bool)> &callback) {
std::cout<<"planAndExecute."<<std::endl;
lock.lock();
std::cout<<"got lock."<<std::endl;
lastTarget = pose;
lastCallback = callback;
moveGroup.setMaxVelocityScalingFactor(currentSpeed);
moveGroup.setMaxAccelerationScalingFactor(1);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if(!success){
callback(false);
return;
}
std::cout<<"Starting planning"<<std::endl;
std::thread([this,callback,pose](){
moveGroup.setStartStateToCurrentState();
moveGroup.setPoseTarget(*pose);
moveGroup.setGoalJointTolerance(0.01);
moveGroup.setGoalOrientationTolerance(0.01);
std::cout<<"Parameters set."<<std::endl;
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveGroup.plan(plan);
if(moveGroup.plan(plan)==moveit::planning_interface::MoveItErrorCode::SUCCESS){
std::cout<<"Planned a path."<<std::endl;
}else{
std::cout<<"Error during planning."<<std::endl;
lock.unlock();
callback(false);
}
if(cancelled){
lastTarget = nullptr;
lock.unlock();
return;
}
bool success = this->moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
bool success = moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
if(!cancelled) {
callback(success);
}

2
src/iisy_config_2/config/moveit_controllers.yaml
View File

@ -1,4 +1,6 @@
# MoveIt uses this configuration for controller management
trajectory_execution:
allowed_execution_duration_scaling: 1.2 # Wait for moves to finish without timeout
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

14
src/moveit_test/src/test.cpp
View File

@ -21,14 +21,18 @@ int main(int argc, char * argv[])
// Set a target Pose
auto const target_pose = []{
geometry_msgs::msg::Pose msg;
msg.orientation.w = 1.0;
msg.position.x = 0.25;
msg.position.y = 0.25;
msg.position.z = 1.25;
msg.orientation.w = 0.0;
msg.orientation.x = 0.0;
msg.orientation.y = 1.0;
msg.orientation.z = 0.0;
msg.position.x = -0.043357;
msg.position.y = -0.281366;
msg.position.z = 1.1;
return msg;
}();
move_group_interface.setGoalOrientationTolerance(100000);
move_group_interface.setGoalOrientationTolerance(0.01);
move_group_interface.setGoalPositionTolerance(0.01);
move_group_interface.setPoseTarget(target_pose);
//std::map<std::string,double> states = {