diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/move.xml b/robossembler/behavior_trees_xml/atomic_skills_xml/move.xml
index aec2835..5b4d3f6 100644
--- a/robossembler/behavior_trees_xml/atomic_skills_xml/move.xml
+++ b/robossembler/behavior_trees_xml/atomic_skills_xml/move.xml
@@ -1,7 +1,7 @@
-
+
\ No newline at end of file
diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp
index 83d7edb..b2a74e7 100644
--- a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp
+++ b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp
@@ -1,6 +1,7 @@
#pragma once
#include
+#include
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
@@ -19,20 +20,36 @@ namespace task_planner
const BT::NodeConfiguration &conf);
void resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg);
-
+ /**
+ * @brief force end of action
+ */
virtual void halt();
+
+ /**
+ * @brief the main body of the node's action until
+ * its completion with a return BT::NodeStatus::SUCCESS
+ * @return BT::NodeStatus
+ */
BT::NodeStatus tick();
+ /**
+ * @brief provides the node with the user
+ * arguments declared in the action.xml
+ * @return BT::PortsList
+ */
static BT::PortsList providedPorts()
{
- return {BT::InputPort("goal")};
+ return {
+ BT::InputPort("arm_group"),
+ BT::InputPort("goal")
+ };
}
private:
std::map waypoints_;
+ std::vector available_groups_;
std::string arm_group_;
std::atomic_bool _halt_requested;
- int counter_;
};
-} // namespace rasms_manipulation
\ No newline at end of file
+} // namespace task_planner
\ No newline at end of file
diff --git a/robossembler/launch/robossembler_bringup.launch.py b/robossembler/launch/robossembler_bringup.launch.py
index 7c033d2..d1b6903 100644
--- a/robossembler/launch/robossembler_bringup.launch.py
+++ b/robossembler/launch/robossembler_bringup.launch.py
@@ -34,6 +34,13 @@ def generate_launch_description():
description="Robot name"
)
)
+ launch_args.append(
+ DeclareLaunchArgument(
+ name="plansys2",
+ default_value="true",
+ description="enable plansys2"
+ )
+ )
# get xacro file path
xacro_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","rasmt.xacro")
@@ -92,7 +99,8 @@ def generate_launch_description():
])
), launch_arguments=[
("robot_description", robot_description_content)
- ]
+ ],
+ condition=IfCondition(LaunchConfiguration("plansys2"))
)
launch_nodes = []
diff --git a/robossembler/launch/task_planner.launch.py b/robossembler/launch/task_planner.launch.py
index fb40cbc..984f4a4 100644
--- a/robossembler/launch/task_planner.launch.py
+++ b/robossembler/launch/task_planner.launch.py
@@ -89,13 +89,13 @@ def generate_launch_description():
}
])
- transport_1 = Node(
- package='robossembler',
- executable='move_action_node',
- name='transport_1',
- namespace=namespace,
- output='screen',
- parameters=[])
+ # transport_1 = Node(
+ # package='robossembler',
+ # executable='move_action_node',
+ # name='transport_1',
+ # namespace=namespace,
+ # output='screen',
+ # parameters=[])
ld = LaunchDescription()
diff --git a/robossembler/pddl/commands b/robossembler/pddl/commands
index 88e1e9b..147f8f8 100644
--- a/robossembler/pddl/commands
+++ b/robossembler/pddl/commands
@@ -1,4 +1,4 @@
-set instance rasms_arm_group robot
+set instance rasmt_arm_group robot
set instance one zone
-set goal (and(robot_move rasms_arm_group one))
+set goal (and(robot_move rasmt_arm_group one))
run
\ No newline at end of file
diff --git a/robossembler/pddl/domain.pddl b/robossembler/pddl/domain.pddl
index be9ec45..17984a4 100644
--- a/robossembler/pddl/domain.pddl
+++ b/robossembler/pddl/domain.pddl
@@ -22,7 +22,7 @@
;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(:durative-action move
:parameters (?r - robot ?z - zone)
- :duration ( = ?duration 5)
+ :duration ( = ?duration 1)
:effect (and (at end(robot_move ?r ?z)))
)
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp
index 1fe4bbf..419902d 100644
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp
+++ b/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp
@@ -18,7 +18,7 @@ namespace task_planner
Move::Move(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf)
- : ActionNodeBase(xml_tag_name, conf), counter_(0)
+ : ActionNodeBase(xml_tag_name, conf)
{
rclcpp::Node::SharedPtr node;
config().blackboard->get("node", node);
@@ -36,9 +36,10 @@ namespace task_planner
if (node->has_parameter("arm_group"))
{
- std::vector group;
- node->get_parameter_or("arm_group", group, {"rasms_arm_group"});
- arm_group_ = group.at(0);
+ node->get_parameter_or("arm_group", available_groups_, {});
+ }else{
+
+ RCLCPP_WARN(LOGGER, "No arm_group names provided");
}
if (node->has_parameter("waypoints"))
@@ -95,7 +96,7 @@ namespace task_planner
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);
+ auto move_group_node = rclcpp::Node::make_shared("robossembler_move_group_interface", node_options);
config().blackboard->get("node", move_group_node);
@@ -106,11 +107,25 @@ namespace task_planner
std::string goal;
getInput("goal", goal);
+ std::string arm_group;
+ getInput("arm_group", arm_group);
+
+ if (std::find(available_groups_.begin(), available_groups_.end(), arm_group) != available_groups_.end())
+ {
+ arm_group_ = arm_group;
+ RCLCPP_INFO(LOGGER, "Provided available move_group [%s]", arm_group.c_str());
+ }else{
+ RCLCPP_WARN(LOGGER, "Provided not allowed move_group name [%s]", arm_group.c_str());
+ }
+
geometry_msgs::msg::Pose pose2moveit;
if (waypoints_.find(goal) != waypoints_.end())
{
pose2moveit = waypoints_[goal];
- RCLCPP_INFO(LOGGER, "Read goal [%f, %f, %f]", pose2moveit.position.x, pose2moveit.position.y, pose2moveit.position.z);
+ RCLCPP_INFO(LOGGER, "Read goal (%s) pose:[%f, %f, %f] orientation:[%f, %f, %f, %f]", goal.c_str(),
+ pose2moveit.position.x, pose2moveit.position.y, pose2moveit.position.z,
+ pose2moveit.orientation.x, pose2moveit.orientation.y, pose2moveit.orientation.z,
+ pose2moveit.orientation.w);
} else {
RCLCPP_WARN(LOGGER, "No coordinate for waypoint [%s]", goal.c_str());
}
@@ -134,7 +149,10 @@ namespace task_planner
goal_pos.position.y = pose2moveit.position.y;
goal_pos.position.z = pose2moveit.position.z;
- RCLCPP_INFO(LOGGER, "Move started to goal [%f, %f, %f]", goal_pos.position.x, goal_pos.position.y, goal_pos.position.z);
+ RCLCPP_INFO(LOGGER, "Move to goal pose:[%f, %f, %f] orientation:[%f, %f, %f, %f] has started",
+ pose2moveit.position.x, pose2moveit.position.y, pose2moveit.position.z,
+ pose2moveit.orientation.x, pose2moveit.orientation.y, pose2moveit.orientation.z,
+ pose2moveit.orientation.w);
move_group_interface.setPoseTarget(goal_pos);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
diff --git a/robossembler/src/move_controller_node.cpp b/robossembler/src/move_controller_node.cpp
index fcfbe6a..a6c43dc 100644
--- a/robossembler/src/move_controller_node.cpp
+++ b/robossembler/src/move_controller_node.cpp
@@ -68,7 +68,7 @@ public:
void step()
{
- problem_expert_->setGoal(plansys2::Goal("(and(robot_move rasmt one))"));
+ problem_expert_->setGoal(plansys2::Goal("(and(robot_move rasmt_arm_group one))"));
auto domain = domain_expert_->getDomain();
auto problem = problem_expert_->getProblem();
@@ -119,6 +119,7 @@ int main(int argc, char ** argv)
rate.sleep();
rclcpp::spin_some(node->get_node_base_interface());
+ node->step();
rate.sleep();
rclcpp::shutdown();