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

Removed cruft, added area parameter

Browse Source
This commit is contained in:
Bastian Hofmann 2023-03-24 13:50:08 +00:00
parent 65df5d10c4
commit b8112a36b3
17 changed files with 110 additions and 505 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/btree_nodes/CMakeLists.txt
View File

@ -42,7 +42,6 @@ set(CPP_FILES
src/nodes/AmICalled.cpp
src/nodes/GenerateXYPose.cpp
src/nodes/MoveActorToTarget.cpp
src/helpers/MinimalSubscriber.cpp
src/nodes/RobotMove.cpp
src/nodes/MoveConnection.cpp
src/nodes/OffsetPose.cpp

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

@ -7,67 +7,58 @@
namespace BT {
template<>
Position2D convertFromString(StringView str) {
printf("Converting string: \"%s\"\n", str.data());
auto split = splitString(str, ';');
std::shared_ptr<Position2D> convertFromString(StringView str) {
auto split = splitString(str, ',');
if (split.size() != 2) {
throw RuntimeError("2 Arguments required.");
throw RuntimeError("Incorrect number of arguments, expected 2 in format '<x>,<y>'.");
}
Position2D pos{};
pos.x = convertFromString<double>(split[0]);
pos.y = convertFromString<double>(split[1]);
auto pos = std::make_shared<Position2D>();
pos->x = convertFromString<double>(split[0]);
pos->y = convertFromString<double>(split[1]);
return pos;
}
template<>
Area convertFromString(StringView str) {
printf("Converting string: \"%s\"\n", str.data());
std::shared_ptr<Area> convertFromString(StringView str) {
auto parts = splitString(str, '|');
if (parts.size() < 3) {
throw RuntimeError("Not enough argument pairs.");
throw RuntimeError("Incorrect number of arguments, expected at least 3 in format '<x>,<y>,<z>|<x>,<y>,<z>|<x>,<y>,<z>'.");
}
Area output{};
auto output = std::make_shared<Area>();
std::vector<Position2D> array(parts.size());
for (unsigned long i = 0; i < parts.size(); ++i) {
array[i] = convertFromString<Position2D>(parts[i]);
array[i] = *convertFromString<std::shared_ptr<Position2D>>(parts[i]);
}
output.triangles = array;
output->triangles = array;
return output;
}
template<>
std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
std::cout << "[ generating pose from string ]" << std::endl;
auto parts = splitString(str, '|');
auto parts = splitString(str, ',');
if (parts.size() != 3) {
throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>|<y>|<z>'.");
throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
}
auto pose = std::make_shared<geometry_msgs::msg::Pose>();
pose->orientation.w = 0;
pose->orientation.w = 1;
pose->orientation.x = 0;
pose->orientation.y = 1;
pose->orientation.y = 0;
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;
return pose;
}
template<>
std::shared_ptr<geometry_msgs::msg::Point> convertFromString(StringView str) {
auto parts = splitString(str, '|');
auto parts = splitString(str, ',');
if (parts.size() != 3) {
throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>|<y>|<z>'.");
throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
}
auto point = std::make_shared<geometry_msgs::msg::Point>();
@ -80,9 +71,9 @@ namespace BT {
template<>
std::shared_ptr<geometry_msgs::msg::Quaternion> convertFromString(StringView str) {
auto parts = splitString(str, '|');
auto parts = splitString(str, ',');
if (parts.size() != 4) {
throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>|<x>|<y>|<z>'.");
throw RuntimeError("Incorrect number of arguments, expected 4 in format '<w>,<x>,<y>,<z>'.");
}
auto point = std::make_shared<geometry_msgs::msg::Quaternion>();

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

@ -1,6 +1,8 @@
#include "Area.h"
#include <behaviortree_cpp_v3/basic_types.h>
#include <behaviortree_cpp_v3/blackboard.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <random>
#include <rclcpp/utilities.hpp>
@ -10,7 +12,6 @@
#include "nodes/GenerateXYPose.h"
#include "nodes/MoveActorToTarget.h"
#include "nodes/RobotMove.h"
#include "helpers/MinimalSubscriber.h"
#include "nodes/OffsetPose.h"
#include "nodes/RandomFailure.h"
#include "nodes/InAreaTest.h"
@ -24,9 +25,12 @@
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("tree_general_node", node_options);
mainNode -> declare_parameter("trees",std::vector<std::string>({}));
mainNode -> declare_parameter("areas",std::vector<std::string>({}));
auto blackboard = Blackboard::create();
auto blackboardMutex = std::mutex();
@ -34,50 +38,6 @@ int main(int argc, char **argv) {
BehaviorTreeFactory factory;
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 }
});
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 }
});
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 }
});
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 }
});
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 }
});
std::cout << "Registering nodes." << std::endl;
factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
@ -120,31 +80,36 @@ int main(int argc, char **argv) {
});
};
factory.registerBuilder<ActorMovement>("ActorMovement",builderActorMovement);
executor.add_node(actorMovementNode);
executor.add_node(actorMovementNode);
bool called = false;
auto IsCalled = [&called](__attribute__((unused)) TreeNode& parent_node) -> NodeStatus{
return called ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
};
auto SetCalledTo = [&called](TreeNode& parent_node) -> NodeStatus{
auto SetCalledTo = [&called,&mainNode](TreeNode& parent_node) -> NodeStatus{
if(!parent_node.getInput<bool>("state",called)){
std::cout<<"no state supplied."<<std::endl;
RCLCPP_DEBUG_STREAM(mainNode->get_logger(),"no state supplied.");
return NodeStatus::FAILURE;
}
std::cout<<"state changed"<<std::endl;
RCLCPP_DEBUG_STREAM(mainNode->get_logger(),"state changed.");
return NodeStatus::SUCCESS;
};
factory.registerSimpleCondition("IsCalled",IsCalled);
factory.registerSimpleAction("SetCalledTo",SetCalledTo,{InputPort("state","state to set called to")});
std::cout << "Creating tree." << std::endl;
mainNode -> declare_parameter("trees",std::vector<std::string>({
"/home/ros/workspace/src/btree_nodes/actorTree.xml",
"/home/ros/workspace/src/btree_nodes/robotTree.xml"
}));
RCLCPP_DEBUG_STREAM(mainNode->get_logger(), "Creating tree.");
auto areaDefinitions = mainNode -> get_parameter("areas").as_string_array();
if(areaDefinitions.size()%2==1){
RCLCPP_ERROR_STREAM(mainNode->get_logger(), "Expected area syntax is \"name\",\"values\"");
return -1;
}
for (unsigned long index = 0; index<areaDefinitions.size(); index+=2) {
RCLCPP_INFO_STREAM(mainNode->get_logger(), "Got area \""<< areaDefinitions[index] <<"\"");
blackboard->set(areaDefinitions[index], BT::convertFromString<std::shared_ptr<Area>>(areaDefinitions[index+1]));
}
auto treeFileNames = mainNode -> get_parameter("trees").as_string_array();
auto trees = std::vector<std::shared_ptr<BT::Tree>>();
@ -156,23 +121,18 @@ int main(int argc, char **argv) {
init->position.x = 6.0;
init->position.y = 6.0;
blackboard->set("safeArea",safeArea);
blackboard->set("warningArea",warningArea);
blackboard->set("unsafeArea",unsafeArea);
blackboard->set("negativeYTable",negativeYTable);
blackboard->set("positiveYTable",positiveYTable);
blackboard->set("actorPos", init);
std::cout << "Starting subscriber." << std::endl;
RCLCPP_INFO(mainNode->get_logger(), "Starting subscriber.");
std::thread([&executor]() {
std::thread([&executor,&mainNode]() {
while(rclcpp::ok()){
executor.spin();
}
std::cout << "Executor stopped." << std::endl;
RCLCPP_WARN(mainNode->get_logger(), "Executor stopped.");
}).detach();
std::cout << "Looping." << std::endl;
RCLCPP_INFO(mainNode->get_logger(), "Looping.");
while (rclcpp::ok()) {
blackboardMutex.lock();
@ -182,7 +142,7 @@ int main(int argc, char **argv) {
blackboardMutex.unlock();
}
std::cout << "End." << std::endl;
RCLCPP_WARN(mainNode->get_logger(), "End.");
rclcpp::shutdown();

6
src/btree_nodes/src/helpers/MinimalSubscriber.cpp
View File

@ -1,6 +0,0 @@
//
// Created by bastian on 26.03.22.
//
#include "MinimalSubscriber.h"

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

@ -1,45 +0,0 @@
//
// Created by bastian on 26.03.22.
//
#ifndef BUILD_MINIMALSUBSCRIBER_H
#define BUILD_MINIMALSUBSCRIBER_H
#include <memory>
#include <geometry_msgs/msg/pose.hpp>
#include "rclcpp/rclcpp.hpp"
template<typename T>
class MinimalSubscriber : public rclcpp::Node {
public:
MinimalSubscriber(const std::string &node_name, const std::string &topic_name,
std::function<void(T)> callback);
public:
static std::thread createAsThread(const std::string &node_name, const std::string &topic_name,
std::function<void(T)> callback);
private:
typename rclcpp::Subscription<T>::SharedPtr subscription_;
};
template<typename T>
MinimalSubscriber<T>::MinimalSubscriber(const std::string &node_name, const std::string &topic_name,
std::function<void(T)> callback) :
Node(node_name) {
this->subscription_ = this->create_subscription<T>(
topic_name, 10, [callback](const T result) {
callback(result);
});
}
template<typename T>
std::thread MinimalSubscriber<T>::createAsThread(const std::string &node_name, const std::string &topic_name,
std::function<void(T)> callback) {
auto subscriber = std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback);
return std::thread([subscriber]() {
rclcpp::spin(subscriber);
});
}
#endif //BUILD_MINIMALSUBSCRIBER_H

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

@ -25,7 +25,7 @@ NodeStatus WeightedRandomNode::tick() {
return NodeStatus::FAILURE;
}
std::regex rgx(";");
std::regex rgx(",");
std::sregex_token_iterator iter(weightString.begin(),
weightString.end(),
rgx,

13
src/moveit_test/CMakeLists.txt → src/btree_trees/CMakeLists.txt
View File

@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.8)
project(moveit_test)
project(btree_trees)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
@ -7,20 +7,11 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(geometry_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(move src/test.cpp)
set_property(TARGET move PROPERTY CXX_STANDARD 17)
ament_target_dependencies(move rclcpp moveit_ros_planning_interface geometry_msgs)
install(TARGETS
move
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY trees DESTINATION share/${PROJECT_NAME})
if(BUILD_TESTING)
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>

2
src/btree_nodes/actorTree.xml → src/btree_trees/trees/actorTree.xml
View File

@ -9,7 +9,7 @@
</Inverter>
<Sequence>
<Control ID="WeightedRandom" weights="100;5;2">
<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}"/>

4
src/btree_nodes/robotTree.xml → src/btree_trees/trees/robotTree.xml
View File

@ -14,11 +14,11 @@
<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="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
<Action ID="RobotMove" target="{target}"/>
<Action ID="RandomFailure" failureChance="0.15"/>
<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="RandomFailure" failureChance="0.05"/>
</Control>

36
src/controller_test/CMakeLists.txt
View File

@ -1,36 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(controller_test)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
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
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_executable(move src/test.cpp)
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)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

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>

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

@ -1,196 +0,0 @@
#include <memory>
#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;
bool common_goal_accepted = false;
rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN;
int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
void common_goal_response(
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) {
common_goal_accepted = false;
printf("Goal rejected\n");
} else {
common_goal_accepted = true;
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());
common_resultcode = result.code;
common_action_result_code = result.result->error_code;
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(
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(1));
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] = 0.0;
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::bind(common_goal_response, std::placeholders::_1);
opt.result_callback = std::bind(common_result_response, std::placeholders::_1);
opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2);
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 :)");
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;
}

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

@ -52,6 +52,51 @@ def generate_launch_description():
executable='ros_actor_action_server',
output='both',
),
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(
PythonLaunchDescriptionSource(

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

@ -1,8 +1,4 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from src/iisy_config/urdf/iisy.urdf | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="iisy">
<link name="world"/>
<link name="base_link"/>

23
src/moveit_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>moveit_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>

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

@ -1,61 +0,0 @@
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
int main(int argc, char * argv[])
{
// Initialize ROS and create the Node
rclcpp::init(argc, argv);
auto const node = std::make_shared<rclcpp::Node>(
"hello_moveit",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
// Create a ROS logger
auto const logger = rclcpp::get_logger("hello_moveit");
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "arm");
// Set a target Pose
auto const target_pose = []{
geometry_msgs::msg::Pose msg;
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(0.01);
move_group_interface.setGoalPositionTolerance(0.01);
move_group_interface.setPoseTarget(target_pose);
//std::map<std::string,double> states = {
// {"base_rot",10.0}
//};
//move_group_interface.setJointValueTarget(states);
// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan msg;
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
// Execute the plan
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planing failed!");
}
// Shutdown ROS
rclcpp::shutdown();
return 0;
}