diff --git a/rasms_manipulator/.vscode/settings.json b/rasms_manipulator/.vscode/settings.json
index 9507a65..910e5e8 100644
--- a/rasms_manipulator/.vscode/settings.json
+++ b/rasms_manipulator/.vscode/settings.json
@@ -11,5 +11,14 @@
"/opt/ros/foxy/lib/python3.8/site-packages"
],
"ros.distro": "foxy",
- "C_Cpp.errorSquiggles": "Disabled"
+ "C_Cpp.errorSquiggles": "Disabled",
+ "files.associations": {
+ "unordered_map": "cpp",
+ "unordered_set": "cpp"
+ },
+ "editor.maxTokenizationLineLength": 2000,
+ "editor.peekWidgetDefaultFocus": "editor",
+ "editor.find.seedSearchStringFromSelection": "never",
+ "diffEditor.maxComputationTime": 500,
+ "diffEditor.maxFileSize": 25
}
\ No newline at end of file
diff --git a/rasms_manipulator/behavior_trees/move.xml b/rasms_manipulator/behavior_trees_xml/move.xml
similarity index 59%
rename from rasms_manipulator/behavior_trees/move.xml
rename to rasms_manipulator/behavior_trees_xml/move.xml
index 3cd0226..25a7ebb 100644
--- a/rasms_manipulator/behavior_trees/move.xml
+++ b/rasms_manipulator/behavior_trees_xml/move.xml
@@ -1,7 +1,7 @@
-
-
+
+
\ No newline at end of file
diff --git a/rasms_manipulator/config/params.yaml b/rasms_manipulator/config/params.yaml
index 50dbf02..319a616 100644
--- a/rasms_manipulator/config/params.yaml
+++ b/rasms_manipulator/config/params.yaml
@@ -1,7 +1,8 @@
-move:
+move_1:
ros_parameters:
- waypoints: ["zero", "one"]
+ plugins:
+ - plansys2_move_bt_node
+ waypoints: ["zero"]
waypoints_coords:
- zero: [0.0, -2.0, 0.0]
- one: [1.8, 0.0, 0.0]
+ zero: [0.0, -2.0, 0.0, 0.0]
\ No newline at end of file
diff --git a/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp
index 0337a65..b4d6225 100644
--- a/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp
+++ b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp
@@ -14,32 +14,24 @@
#include "moveit_msgs/msg/place_location.hpp"
#include "moveit_msgs/msg/move_it_error_codes.hpp"
#include "moveit_msgs/action/move_group.hpp"
-
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-#include "tf2/transform_datatypes.h"
-#include "tf2/LinearMath/Transform.h"
-#include "tf2_msgs/msg/tf_message.hpp"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/create_timer_ros.h"
-#include "tf2_ros/transform_broadcaster.h"
-#include "tf2_ros/transform_listener.h"
+#include "moveit_msgs/action/"
#include
-#include
#include
namespace rasms_manipulation
{
-class Move: public BT::ActionNodeBase
+class Move: public plansys2::BtActionNode
{
public:
explicit Move(
const std::string& xml_tag_name,
+ const std::string& action_name,
const BT::NodeConfiguration& conf);
- void halt();
- BT::NodeStatus on_tick();
+ void on_tick() override;
+ BT::NodeStatus on_success() override;
geometry_msgs::msg::PoseStamped pose2BaseFootprint(geometry_msgs::msg::PoseStamped input);
void resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg);
@@ -47,19 +39,13 @@ public:
static BT::PortsList providedPorts()
{
return {
- BT::InputPort("goal")
+ BT::InputPort("goal")
};
}
private:
- rclcpp::Node::SharedPtr node_;
- rclcpp::Publisher::SharedPtr move_pub_;
- rclcpp::Subscription::SharedPtr result_sub_;
- std::map places_;
- std::shared_ptr tf_listener_;
- std::shared_ptr tf_buffer_;
- int result_;
- bool move_action_sent_;
+ int goal_reached_;
+ std::map waypoints_;
};
} //namespace rasms_manipulator
\ No newline at end of file
diff --git a/rasms_manipulator/src/assemble_action_node.cpp b/rasms_manipulator/src/assemble_action_node.cpp
new file mode 100644
index 0000000..f80c832
--- /dev/null
+++ b/rasms_manipulator/src/assemble_action_node.cpp
@@ -0,0 +1,68 @@
+// Copyright 2019 Intelligent Robotics Lab
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+#include
+
+#include "plansys2_executor/ActionExecutorClient.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+using namespace std::chrono_literals;
+
+class AssembleAction : public plansys2::ActionExecutorClient
+{
+public:
+ AssembleAction()
+ : plansys2::ActionExecutorClient("assemble", 500ms)
+ {
+ progress_ = 0.0;
+ }
+
+private:
+ void do_work()
+ {
+ if (progress_ < 1.0) {
+ progress_ += 0.05;
+ send_feedback(progress_, "Assemble running");
+ } else {
+ finish(true, 1.0, "Assemble completed");
+
+ progress_ = 0.0;
+ std::cout << std::endl;
+ }
+
+ std::cout << "\r\e[K" << std::flush;
+ std::cout << "Assemble ... [" << std::min(100.0, progress_ * 100.0) << "%] " <<
+ std::flush;
+ }
+
+ float progress_;
+};
+
+int main(int argc, char ** argv)
+{
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared();
+
+ node->set_parameter(rclcpp::Parameter("action_name", "assemble"));
+ node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+
+ rclcpp::spin(node->get_node_base_interface());
+
+ rclcpp::shutdown();
+
+ return 0;
+}
\ No newline at end of file
diff --git a/rasms_manipulator/src/assemble_controller_node.cpp b/rasms_manipulator/src/assemble_controller_node.cpp
new file mode 100644
index 0000000..a136e64
--- /dev/null
+++ b/rasms_manipulator/src/assemble_controller_node.cpp
@@ -0,0 +1,116 @@
+// Copyright 2019 Intelligent Robotics Lab
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+
+#include
+
+#include "plansys2_msgs/msg/action_execution_info.hpp"
+#include "plansys2_msgs/msg/plan.hpp"
+
+#include "plansys2_domain_expert/DomainExpertClient.hpp"
+#include "plansys2_executor/ExecutorClient.hpp"
+#include "plansys2_planner/PlannerClient.hpp"
+#include "plansys2_problem_expert/ProblemExpertClient.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+class Assemble : public rclcpp::Node
+{
+public:
+ Assemble()
+ : rclcpp::Node("assembling_controller")
+ {
+ }
+
+ bool init()
+ {
+ domain_expert_ = std::make_shared();
+ planner_client_ = std::make_shared();
+ problem_expert_ = std::make_shared();
+ executor_client_ = std::make_shared();
+
+ //init_knowledge();
+
+ auto domain = domain_expert_->getDomain();
+ auto problem = problem_expert_->getProblem();
+ auto plan = planner_client_->getPlan(domain, problem);
+
+ if (!plan.has_value())
+ {
+ std::cout << "Could not find plan to reach goal " <<
+ parser::pddl::toString(problem_expert_->getGoal()) << std::endl;
+ return false;
+ }
+
+ if (!executor_client_->start_plan_execution(plan.value()))
+ {
+ RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)");
+ }
+
+ return true;
+ }
+
+ void init_knowledge()
+ {
+ problem_expert_->addInstance(plansys2::Instance{"rasms", "robot"});
+ problem_expert_->setGoal(plansys2::Goal("robot moved"));
+ }
+
+ void step()
+ {
+ if (!executor_client_->execute_and_check_plan())
+ { // Plan finished
+ auto result = executor_client_->getResult();
+
+ if (result.value().success)
+ {
+ RCLCPP_INFO(get_logger(), "Plan succesfully finished");
+ } else {
+ RCLCPP_ERROR(get_logger(), "Plan finished with error");
+ }
+ }
+ }
+
+private:
+ std::shared_ptr domain_expert_;
+ std::shared_ptr planner_client_;
+ std::shared_ptr problem_expert_;
+ std::shared_ptr executor_client_;
+};
+
+int main(int argc, char ** argv)
+{
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared();
+
+ if (!node->init())
+ {
+ return 0;
+ }
+
+ rclcpp::Rate rate(5);
+ while (rclcpp::ok())
+ {
+ node->step();
+
+ rate.sleep();
+ rclcpp::spin_some(node->get_node_base_interface());
+ }
+
+ rclcpp::shutdown();
+
+ return 0;
+}
\ No newline at end of file
diff --git a/rasms_manipulator/src/behavior_tree_nodes/Move.cpp b/rasms_manipulator/src/behavior_tree_nodes/Move.cpp
index 8b23e4e..d1c86b2 100644
--- a/rasms_manipulator/src/behavior_tree_nodes/Move.cpp
+++ b/rasms_manipulator/src/behavior_tree_nodes/Move.cpp
@@ -3,113 +3,133 @@
#include "rasms_bt/behavior_tree_nodes/Move.hpp"
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
namespace rasms_manipulation
{
Move::Move(
- const std::string& xml_tag_name,
+ const std::string& xml_tag_name,
+ const std::string& action_name,
const BT::NodeConfiguration& conf)
- : BT::ActionNodeBase(xml_tag_name, conf)
+ : plansys::BtActionNode(xml_tag_name, action_name, conf)
{
- node_ = rclcpp::Node::make_shared("move_action_comms");
- move_pub_ = node_->create_publisher("/moveit/place", 1);
- result_sub_ = node_->create_subscription(
- "/moveit/result",
- 1,
- std::bind(&Move::resultCallback, this, std::placeholders::_1));
- rclcpp::Node::SharedPtr bt_node;
- config().blackboard->get("node", bt_node);
+ rclcpp::Node::SharedPtr node;
+ config().blackboard->get("node", node);
- tf_buffer_ = std::make_shared(node_->get_clock());
- auto timer_interface = std::make_shared(
- node_->get_node_base_interface(),
- node_->get_node_timers_interface());
- tf_buffer_->setCreateTimerInterface(timer_interface);
- tf_listener_ = std::make_shared(*tf_buffer_);
-
- move_action_sent_ = false;
-}
-
-void Move::halt()
-{
- std::cout << "Move halt" << std::endl;
-}
-
-void Move::resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg)
-{
- result_ = msg->val;
-}
-
-double getYaw(geometry_msgs::msg::Quaternion mQ)
-{
- tf2::Quaternion tQ(
- mQ.x,
- mQ.y,
- mQ.z,
- mQ.w);
- tf2::Matrix3x3 m(tQ);
- double roll, pitch, yaw;
- m.getRPY(roll, pitch, yaw);
- return yaw;
-}
-
-geometry_msgs::msg::PoseStamped Move::pose2BaseFootprint(geometry_msgs::msg::PoseStamped input)
-{
- tf2::Transform frame2bf, obj2frame, bf2Object;
- obj2frame.setOrigin({input.pose.posetion.x,
- input.pose.position.y,
- input.pose.position.z});
- obj2frame.setRotation({input.pose.posetion.x,
- input.pose.position.y,
- input.pose.position.z,
- input.pose.position.w});
- geometry_msgs::msg::PoseStamped object_pose;
try
{
- auto tf = tf_buffer_->lookupTransform(
- input.header.frame_id, "base", tf2::TimePointZero);
- tf2::fromMsg(tf.transform, frame2bf);
- bf2Object = obj2frame * frame2bf;
+ node->declare_parameter("waypoints");
+ node->declare_parameter("waypoint_coords");
+ }
+ catch(const rclcpp::exceptions::ParameterAlreadyDeclaredException& ex) {
+ std::cerr << ex.what() << '\n';
+ }
+
+ if (node->has_parameter("waypoints"))
+ {
+ std::vector wp_names;
+ node->get_parameter_or("waypoints", wp_names, {});
+
+ for(auto& wp: wp_names)
+ {
+ try
+ {
+ node->declare_parameter("waypoint_coords." + wp);
+ } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException& ex) {
+ std::cerr << ex.what() << '\n';
+ }
+
+ std::vector coords;
+ if (node->get_parameter_or("waypoint_coords." + wp, coords, {}))
+ {
+ geometry_msgs::msg::Pose pose;
+ pose.position.x = coords[0];
+ pose.position.y = coords[1];
+ pose.position.z = coords[2];
+ pose.position.w = coords[3]
+ // pose.orientation.x = coords[3];
+ // pose.orientation.y = coords[4];
+ // pose.orientation.z = coords[5];
+ // pose.orientation.w = coords[6];
+
+ waypoints_[wp] = pose;
+ } else {
+ std::cerr << "No coordinate configured for waypoint [" << wp << "]" << std::endl;
+ }
+ }
}
}
-BT::NodeStatus Move::on_tick()
+void Move::on_tick()
{
- if (!move_action_sent_)
- {
- geometry_msgs::msg::PoseStamped goal;
- getInput("goal", goal);
- RCLCPP_INFO(node_->get_logger(), "Move to: x:%f y:%f z:%f yaw:%f",
- goal.pose.position.x,
- goal.pose.position.y,
- goal.pose.position.z,
- getYaw(goal.pose.orientation));
- moveit_msgs::msg::PlaceLocation msg;
- msg.place_pose = pose2BaseFootprint(goal);
- move_pub_->publish(msg);
- result_ = 0;
- move_action_sent_ = true;
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("moveit_cpp_move", node_options);
+ config().blackboard->get("node", node);
+
+
+ std::string goal;
+ getInput("goal", goal);
+
+ geometry_msgs::msg::Pose pose2moveit;
+ if (waypoints_.find(goal) != waypoints_.end()) {
+ pose2moveit = waypoints_[goal];
+ } else {
+ std::cerr << "No coordinate for waypoint [" << goal << "]" << std::endl;
}
- rclcpp:spin_some(node_);
+ std::string PLANNING_GROUP = "rasms_arm_group";\
- if (result_ == 0)
+ moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
+ geometry_msgs::msg::PoseStamped goal_pos;
+
+ goal_pos.header.frame_id = "base";
+ goal_pos.pose.position.x = pose2moveit.position.x;
+ goal_pos.pose.position.y = pose2moveit.position.y;
+ goal_pos.pose.position.z = pose2moveit.position.z;
+ goal_pos.pose.position.w = pose2moveit.position.w
+ // goal_pos.pose.orientation = pose2moveit.orientation;
+
+ move_group.setPoseTarget(goal_pose, "link6");
+ moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+ moveit::planning_interface::MoveItErrorCode success = move_group(my_plan);
+ if (success)
{
- return BT::NodeStatus::RUNNING;
- } else if (result_ == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
- {
- result_ = 0;
- RCLCPP_INFO(node_->get_logger(), "Success!");
- return BT::NodeStatus::SUCCESS;
+ move_group.execute(my_plan);
} else {
- RCLCPP_ERROR(node_->get_logger(), "Move error: MoveItErrorCodes[%i]", result_);
- return BT::NodeStatus::FAILURE;
+ return;
}
}
-} //namespace rasms_manipulator
+BT::NodeStatus
+Move::on_success()
+{
+ return BT::NodeStatus::SUCCESS;
+}
+
+
+} // namespace plansys2_bt_tests
+
+#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
- factory.registerNodeType("Move");
+ BT::NodeBuilder builder =
+ [](const std::string & name, const BT::NodeConfiguration & config)
+ {
+ return std::make_unique(
+ name, "navigate_to_pose", config);
+ };
+
+ factory.registerBuilder(
+ "Move", builder);
}
\ No newline at end of file