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

write move action with moveit

Browse source
This commit is contained in:
Splinter1984 2022-01-09 17:56:22 +08:00
parent 21d58b5992
commit 8c6b912e00
7 changed files with 102 additions and 55 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rasms_manipulator/config/params.yaml
View file

@ -5,4 +5,4 @@ move:
waypoints: ["one", "two"]
waypoint_coords:
one: [0.0, 0.0, 0.0]
two: [0.28, -0.2, 0.5]
two: [0.01, 0.01, 0.6]

3
rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp
View file

@ -13,7 +13,7 @@
namespace rasms_manipulation
{
class Move : public BT::AsyncActionNode
class Move : public BT::ActionNodeBase
{
public:
Move(const std::string &xml_tag_name,
@ -32,6 +32,7 @@ namespace rasms_manipulation
private:
std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
std::atomic_bool _halt_requested;
int counter_;
};
} // namespace rasms_manipulation

57
rasms_manipulator/launch/rasms_manipulation.launch.py
View file

@ -1,16 +1,55 @@
import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, "r") as file:
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
# planning_context
robot_description_config = load_file(
"rasms_description", "urdf/rasms_description.urdf.xacro"
)
robot_description = {"robot_description": robot_description_config}
robot_description_semantic_config = load_file(
"rasms_moveit_config", "srdf/rasms_description.srdf"
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_config
}
kinematics_yaml = load_yaml(
"rasms_moveit_config", "config/kinematics.yml"
)
pkg_dir = get_package_share_directory('rasms_manipulator')
namespace = LaunchConfiguration('namespace')
@ -40,16 +79,21 @@ def generate_launch_description():
output='screen',
parameters=[
pkg_dir + '/config/params.yaml',
robot_description,
robot_description_semantic,
kinematics_yaml,
{
'action_name': 'move',
### [Groot](https://github.com/BehaviorTree/Groot) behavior_tree_visual tool
### [](https://intelligentroboticslab.gsyc.urjc.es/ros2_planning_system.github.io/design/index.html)
# need to provide visual information about working behavior tree
# 'publisher_port': 1668,
# 'server_port': 1669,
'bt_xml_file': pkg_dir + '/behavior_trees_xml/move.xml'
}
])
transport_1_cmd = Node(
package='rasms_manipulator',
executable='rasms_action_node',
name='transport_1',
namespace=namespace,
output='screen',
parameters=[]) # Create the launch description and populate
ld = LaunchDescription()
@ -59,5 +103,6 @@ def generate_launch_description():
# Declare the launch options
ld.add_action(plansys2_cmd)
ld.add_action(move_1)
# ld.add_action(transport_1_cmd)
return ld

4
rasms_manipulator/pddl/commands
View file

@ -1,6 +1,6 @@
set instance rasms robot
set instance one zone
set instance two zone
set predicate (robot_at rasms one)
set goal (and(robot_moved rasms one))
set goal (and(robot_at rasms two))
run

71
rasms_manipulator/src/behavior_tree_nodes/Move.cpp
View file

@ -14,9 +14,11 @@
namespace rasms_manipulation
{
static const rclcpp::Logger LOGGER = rclcpp::get_logger("plansys2_move_node");
Move::Move(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf)
: AsyncActionNode(xml_tag_name, conf)
: ActionNodeBase(xml_tag_name, conf), counter_(0)
{
rclcpp::Node::SharedPtr node;
config().blackboard->get("node", node);
@ -28,7 +30,7 @@ namespace rasms_manipulation
}
catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex)
{
std::cerr << ex.what() << '\n';
RCLCPP_WARN(LOGGER, "%c", ex.what());
}
if (node->has_parameter("waypoints"))
@ -56,7 +58,7 @@ namespace rasms_manipulation
}
else
{
std::cerr << "No coordinate configured for waypoint [" << wp << "]" << std::endl;
RCLCPP_WARN(LOGGER, "No coordinate configured for waypoint [%s]", wp.c_str());
}
}
}
@ -65,17 +67,21 @@ namespace rasms_manipulation
void Move::halt()
{
_halt_requested = true;
std::cout << "Move halt" << std::endl;
RCLCPP_INFO(LOGGER, "Move halt");
}
BT::NodeStatus Move::tick()
{
rclcpp::Node::SharedPtr node;
config().blackboard->get("node", node);
// rclcpp::executors::SingleThreadedExecutor executor;
// executor.add_node(node);
// std::thread([&executor]() {executor.spin();}).detach();
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);
config().blackboard->get("node", move_group_node);
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread([&executor]() { executor.spin(); }).detach();
std::string goal;
getInput<std::string>("goal", goal);
@ -84,53 +90,46 @@ namespace rasms_manipulation
if (waypoints_.find(goal) != waypoints_.end())
{
pose2moveit = waypoints_[goal];
std::cout << pose2moveit.position.x << ", " << pose2moveit.position.y << ", " << pose2moveit.position.z << std::endl;
RCLCPP_INFO(LOGGER, "Read goal [%f, %f, %f]", pose2moveit.position.x, pose2moveit.position.y, pose2moveit.position.z);
} else {
RCLCPP_WARN(LOGGER, "No coordinate for waypoint [%s]", goal.c_str());
}
else
{
std::cerr << "No coordinate for waypoint [" << goal << "]" << std::endl;
}
//Setup MoveIt interface for planning
moveit::planning_interface::MoveGroupInterface::Options move_group_options("rasms_arm_group", "/robot_description");
moveit::planning_interface::MoveGroupInterface move_group_interface(node, move_group_options);
// Raw pointers are frequently used to refer to the planning group for improved performance.
// const moveit::core::JointModelGroup* joint_model_group =
// move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
// move_group_interface->setStartState(*move_group_interface->getCurrentState());
static const std::string PLANNING_GROUP = "rasms_arm_group";
RCLCPP_INFO(LOGGER, "MoveGroupInterface set current planning group `%s`", PLANNING_GROUP.c_str());
moveit::planning_interface::MoveGroupInterface::Options move_group_options(PLANNING_GROUP, "robot_description");
moveit::planning_interface::MoveGroupInterface move_group_interface(move_group_node, move_group_options);
moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
move_group_interface.setStartState(start_state);
/*----------
00823 // pose goal;
00824 // for each link we have a set of possible goal locations;
00825 std::map<std::string, std::vector<geometry_msgs::PoseStamped> > pose_targets_;
----------*/
geometry_msgs::msg::Pose goal_pos;
goal_pos.orientation.w = 1.0;
goal_pos.position.x = pose2moveit.position.x;
goal_pos.position.y = pose2moveit.position.y;
goal_pos.position.z = pose2moveit.position.z;
std::cout << "[MOVE_STARTED]: goal [" <<
goal_pos.position.x << ", " <<
goal_pos.position.y << ", " <<
goal_pos.position.z << ", " <<
"]" << std::endl;
RCLCPP_INFO(LOGGER, "Move started to goal [%f, %f, %f]", goal_pos.position.x, goal_pos.position.y, goal_pos.position.z);
move_group_interface.setPoseTarget(goal_pos);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if (success)
{
//RCLCPP_INFO("Planning successful");
// move_group_interface.setStartStateToCurrentState();
RCLCPP_INFO(LOGGER, "Planning success");
move_group_interface.execute(my_plan);
move_group_interface.move();
counter_++;
} else {
std::cout << " failed to generate a plan" << std::endl;
RCLCPP_WARN(LOGGER, "Failed to generate a plan");
return BT::NodeStatus::FAILURE;
}
if (counter_ != waypoints_.size())
return BT::NodeStatus::RUNNING;
return BT::NodeStatus::SUCCESS;
}

10
rasms_manipulator/src/rasms_controller_node.cpp
View file

@ -12,15 +12,17 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <plansys2_pddl_parser/Utils.h>
#include <memory>
#include <random>
#include "plansys2_msgs/msg/action_execution_info.hpp"
#include "plansys2_msgs/msg/plan.hpp"
#include "plansys2_executor/ExecutorClient.hpp"
#include "plansys2_domain_expert/DomainExpertClient.hpp"
#include "plansys2_problem_expert/ProblemExpertClient.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"
@ -36,9 +38,9 @@ public:
void init()
{
domain_expert_ = std::make_shared<plansys2::DomainExpertClient>();
planner_client_ = std::make_shared<plansys2::PlannerClient>();
problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
executor_client_ = std::make_shared<plansys2::ExecutorClient>();
planner_client_ = std::make_shared<plansys2::PlannerClient>();
init_knowledge();

10
rasms_moveit_config/launch/rasms_control.launch.py
View file

@ -86,7 +86,7 @@ def generate_launch_description():
arguments=["rasms_arm_controller", "--controller-manager", "/controller_manager"],
)
"""load_controllers = []
load_controllers = []
for controller in [
"rasms_arm_controller",
"joint_state_broadcaster",
@ -97,17 +97,17 @@ def generate_launch_description():
shell=True,
output="screen",
)
]"""
]
return LaunchDescription(
launch_args +
#load_controllers +
load_controllers +
[
controller_manager,
robot_state_publisher,
joint_state_publisher,
static_tf,
joint_state_broadcaster,
rasms_arm_controller
rasms_arm_controller,
joint_state_publisher
]
)