robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

complete merge

Browse source
This commit is contained in:
Splinter1984 2022-01-19 23:03:56 +08:00
parent 8fc9b379c3
commit c8fb83c7a1
8 changed files with 68 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

2
robossembler/behavior_trees_xml/atomic_skills_xml/move.xml
View file

@ -1,7 +1,7 @@
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<Move name="move" goal="${arg1}"/>
<Move name="move" arm_group="${arg0}" goal="${arg1}"/>
</Sequence>
</BehaviorTree>
</root>

25
robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp
View file

@ -1,6 +1,7 @@
#pragma once
#include <string>
#include <vector>
#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<std::string>("goal")};
return {
BT::InputPort<std::string>("arm_group"),
BT::InputPort<std::string>("goal")
};
}
private:
std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
std::vector<std::string> available_groups_;
std::string arm_group_;
std::atomic_bool _halt_requested;
int counter_;
};
} // namespace rasms_manipulation
} // namespace task_planner

10
robossembler/launch/robossembler_bringup.launch.py
View file

@ -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 = []

14
robossembler/launch/task_planner.launch.py
View file

@ -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()

4
robossembler/pddl/commands
View file

@ -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

2
robossembler/pddl/domain.pddl
View file

@ -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)))
)

32
robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp
View file

@ -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<std::string> 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<std::string>("goal", goal);
std::string arm_group;
getInput<std::string>("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;

3
robossembler/src/move_controller_node.cpp
View file

@ -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();