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

merge actual version

Browse source
This commit is contained in:
Splinter1984 2022-02-12 01:10:57 +08:00
commit 0e316aaf8d
42 changed files with 1084 additions and 727 deletions
Download patch file Download diff file Expand all files Collapse all files

30
rasmt_moveit_config/CMakeLists.txt
View file

@ -1,6 +1,16 @@
cmake_minimum_required(VERSION 3.16.3)
cmake_minimum_required(VERSION 3.5)
project(rasmt_moveit_config)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
@ -8,9 +18,17 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
install(
DIRECTORY config launch
DESTINATION share/${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()
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
ament_package()

38
rasmt_moveit_config/launch/rasmt_moveit.launch.py
View file

@ -156,9 +156,47 @@ def generate_launch_description():
]
)
move_topose_action_server = Node(
package="robossembler_servers",
executable="move_topose_action_server",
name="move_to_pose_moveit",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
robot_description_planning,
ompl_yaml,
planning,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
use_sim_time
]
)
move_to_joint_state_action_server = Node(
package="robossembler_servers",
executable="move_to_joint_states_action_server",
name="joint_states_moveit",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
robot_description_planning,
ompl_yaml,
planning,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
use_sim_time
]
)
launch_nodes = []
launch_nodes.append(rviz)
launch_nodes.append(move_group_node)
launch_nodes.append(move_topose_action_server)
launch_nodes.append(move_to_joint_state_action_server)

4
rasmt_moveit_config/package.xml
View file

@ -9,9 +9,9 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<exec_depend>rviz2</exec_depend>
<!--exec_depend>rviz2</exec_depend>
<exec_depend>moveit_ros_move_group</exec_depend>
<exec_depend>moveit_planners</exec_depend>
<exec_depend>moveit_planners</exec_depend-->
<export>
<build_type>ament_cmake</build_type>

2
rasmt_support/urdf/tools/rasmt_hand_macro.xacro
View file

@ -153,7 +153,7 @@
</visual>
<collision>
<origin
xyz="0 1 0"
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh

17
robossembler/CMakeLists.txt
View file

@ -19,6 +19,7 @@ find_package(plansys2_pddl_parser REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(plansys2_bt_actions REQUIRED)
find_package(gazebo_msgs REQUIRED)
find_package(robossembler_interfaces REQUIRED)
find_package(behavior_tree REQUIRED)
if (NOT CMAKE_CXX_STANDARD)
@ -42,16 +43,21 @@ set(dependencies
ament_index_cpp
plansys2_bt_actions
gazebo_msgs
robossembler_interfaces
behavior_tree
std_msgs
)
include_directories(include)
add_library(robossembler_move_bt_node SHARED src/behavior_tree_nodes/atomic_skills/Move.cpp)
list(APPEND plugin_libs robossembler_move_bt_node)
add_library(robossembler_move_topose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp)
list(APPEND plugin_libs robossembler_move_topose_bt_action_client)
add_library(robossembler_move_gripper_bt_node SHARED src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp)
list(APPEND plugin_libs robossembler_move_gripper_bt_node)
add_library(robossembler_get_entity_state_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp)
list(APPEND plugin_libs robossembler_get_entity_state_bt_action_client)
add_library(robossembler_move_gripper_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp)
list(APPEND plugin_libs robossembler_move_gripper_bt_action_client)
add_library(robossembler_print_bt_node SHARED src/behavior_tree_nodes/Print.cpp)
list(APPEND plugin_libs robossembler_print_bt_node)
@ -64,14 +70,11 @@ endforeach()
add_executable(move_controller_node src/move_controller_node.cpp)
ament_target_dependencies(move_controller_node ${dependencies})
add_executable(move_action_node src/move_action_node.cpp)
ament_target_dependencies(move_action_node ${dependencies})
install(DIRECTORY launch pddl behavior_trees_xml config DESTINATION share/${PROJECT_NAME})
install(TARGETS
move_controller_node
move_action_node
${plugin_libs}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib

7
robossembler/behavior_trees_xml/atomic_skills_xml/EmuPoseEstimation.xml
View file

@ -1,7 +0,0 @@
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<EmuPoseEstimation name="EmuPoseEstimation" model_name="${arg1}"/>
</Sequence>
</BehaviorTree>
</root>

40
robossembler/behavior_trees_xml/atomic_skills_xml/get-part-state.xml Normal file
View file

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GraspPart">
<!-- ////////// -->
<BehaviorTree ID="GraspPart">
<Fallback name="root_Fallback">
<Sequence name="GraspSkill">
<Fallback>
<Condition ID="PartInScene"/>
<Action ID="FindPart" PartName="{arg1}" PartPose="{arg0}"/>
</Fallback>
<Action ID="MoveToPoint" arm_group="${arg2}" goal="${arg0}"/>
<Action ID="GraspPart" arm_group="${arg3}" fingers_wide="${arg4}"/>
<Action ID="MoveToPoint" arm_group="{arg2}" goal="{arg5}"/>
</Sequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="FindPart">
<inout_port name="PartName"/>
<output_port name="PartPose"/>
</Action>
<Action ID="GraspPart">
<input_port name="arm_group"/>
<input_port name="fingers_wide"/>
</Action>
<Action ID="MoveToPoint">
<input_port name="arm_group"/>
<input_port name="goal"/>
</Action>
<Condition ID="PartInScene"/>
<Condition ID="PathFree"/>
<Action ID="SpawnPart">
<inout_port name="PartName"/>
<input_port name="PartPose"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

38
robossembler/behavior_trees_xml/atomic_skills_xml/grasp_skill.xml Normal file
View file

@ -0,0 +1,38 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GraspPart">
<!-- ////////// -->
<BehaviorTree ID="GraspPart">
<Sequence name="GraspSkill">
<Action ID="SpawnPart" PartName="${arg2}" PartPose="${gazebo_entity_name}"/>
<Sequence>
<Condition ID="PartInScene"/>
<Action ID="FindPart" PartName="${arg2}" PartPose="${part_pose}"/>
</Sequence>
<Action ID="Move" arm_group="${arg0}" goal="${part_pose}"/>
<Action ID="GraspPart" arm_group="${arg1}" fingers_wide="${goal1}"/>
<Action ID="Move" arm_group="${arg0}" goal="${goal2}"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="FindPart">
<inout_port name="PartName"/>
<output_port name="PartPose"/>
</Action>
<Action ID="GraspPart">
<input_port name="arm_group"/>
<input_port name="fingers_wide"/>
</Action>
<Action ID="Move">
<input_port name="arm_group"/>
<input_port name="goal"/>
</Action>
<Condition ID="PartInScene"/>
<Condition ID="PathFree"/>
<Action ID="SpawnPart">
<inout_port name="PartName"/>
<input_port name="PartPose"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

28
robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml Normal file
View file

@ -0,0 +1,28 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<Action ID="Print" frame="${arg2}" server_name="/spawn_entity" server_timeout="10"/>
<Action ID="GetEntityState" part_name="${arg2}" server_name="/get_entity_state" server_timeout="10"/>
<Action ID="MoveToPose" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveGripper" arm_group="${arg1}" server_name="/move_to_joint_states" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="GetEntityState">
<input_port name="part_name"/>
</Action>
<Action ID="MoveToPose">
<input_port name="arm_group"/>
</Action>
<Action ID="MoveGripper">
<input_port name="arm_group"/>
</Action>
<Action ID="Print">
<input_port name="frame"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

26
robossembler/config/params.yaml
View file

@ -1,27 +1,9 @@
move:
ros__parameters:
plugins:
- robossembler_move_bt_node
arm_group: ["rasmt_arm_group"]
waypoints: ["one"]
waypoint_coords:
one: [0.01, 0.01, 0.6,
0.0 , 0.0 , 0.0,
1.0]
move_gripper:
ros__parameters:
plugins:
- robossembler_move_gripper_bt_node
gripper_group: ["rasmt_hand_arm_group"]
relative_gaps: ["open", "close"]
gaps:
open: 0.039
close: 0.0
print:
assemble_1:
ros__parameters:
plugins:
- robossembler_get_entity_state_bt_action_client
- robossembler_move_topose_bt_action_client
- robossembler_move_gripper_bt_action_client
- robossembler_print_bt_node
frames: ["cube"]
materials: ["material1", material2]

4
robossembler/include/robot_bt/behavior_tree_nodes/Print.hpp
View file

@ -24,9 +24,7 @@ public:
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("frame"),
BT::InputPort<std::string>("printer"),
BT::InputPort<std::string>("material")
BT::InputPort<std::string>("frame")
});
}

32
robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp
View file

@ -1,32 +0,0 @@
#pragma once
#include <string>
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <gazebo_msgs/srv/get_entity_state.hpp>
#include "plansys2_bt_actions/BTActionNode.hpp"
#include "rclcpp/rclcpp.hpp"
namespace task_planner
{
class EmuPoseEstimation : public BT::ActionNodeBase
{
public:
EmuPoseEstimation(const std::string &xml_tag,
const BT::NodeConfiguration &conf);
void resultCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
virtual void halt() override;
BT::NodeStatus tick() override;
static BT::PortsList providedPorts()
{
return {BT::InputPort<std::string>("model_name")};
}
private:
std::string model_name;
};
}

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

@ -1,53 +0,0 @@
#pragma once
#include <string>
#include <vector>
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose.hpp"
#include "moveit_msgs/msg/move_it_error_codes.hpp"
#include "rclcpp/rclcpp.hpp"
namespace task_planner
{
class Move : public BT::ActionNodeBase
{
public:
Move(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf);
/**
* @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>("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;
};
} // namespace task_planner

54
robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp
View file

@ -1,54 +0,0 @@
#pragma once
#include <string>
#include <vector>
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "moveit_msgs/msg/move_it_error_codes.hpp"
#include "rclcpp/rclcpp.hpp"
namespace task_planner
{
class MoveGripper : public BT::ActionNodeBase
{
public:
MoveGripper(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf);
/**
* @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>("gripper_group"),
BT::InputPort<std::string>("relative_gap")
};
}
private:
const double max_gripper_gap_ = 0.04;
std::map<std::string, double> relative_gaps_;
std::vector<std::string> available_groups_;
std::string gripper_group_;
std::atomic_bool _halt_requested;
};
} // namespace task_planner

20
robossembler/launch/bt_check.launch.py Normal file
View file

@ -0,0 +1,20 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
return LaunchDescription([
Node(
package='robossembler',
namespace='',
executable='bt_check',
# Do not declare a node name otherwise it messes with the action node names and will result in
# duplicate nodes!
parameters=[
{'bt_file_path': os.path.join(get_package_share_directory('robossembler'), 'behavior_trees_xml/atomic_skills_xml/simple_sequence.xml')},
{'plugins': ['robossembler_get_entity_state_bt_action_client', 'robossembler_move_topose_bt_action']}
]
),
])

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

@ -37,7 +37,7 @@ def generate_launch_description():
launch_args.append(
DeclareLaunchArgument(
name="plansys2",
default_value="true",
default_value="false",
description="enable plansys2"
)
)

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

@ -2,8 +2,6 @@ import os
import yaml
import xacro
__debug = True
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
@ -11,6 +9,8 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
__debug = True
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)
@ -54,18 +54,14 @@ def generate_launch_description():
name="namespace",
default_value='',
description='Namespace')
if __debug == False:
declare_robot_description = DeclareLaunchArgument(
name="robot_description",
description="Robot description XML file"
)
"""declare_robot_description = DeclareLaunchArgument(
name="robot_description",
description="Robot description XML file"
)"""
# stdout_linebuf_envvar = SetEnvironmentVariable(
# 'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
robot_description = {"robot_description":LaunchConfiguration("robot_description")}
if __debug:
# get xacro file path
xacro_file = os.path.join(get_package_share_directory("rasmt_support"),"urdf/","rasmt.xacro")
@ -87,10 +83,10 @@ def generate_launch_description():
'namespace': namespace
}.items())
move_1 = Node(
assemble_1 = Node(
package='plansys2_bt_actions',
executable='bt_action_node',
name='move',
name='assemble_1',
namespace=namespace,
output='screen',
parameters=[
@ -99,62 +95,35 @@ def generate_launch_description():
robot_description_semantic,
kinematics_yaml,
{
'action_name': 'move',
'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/move.xml',
'action_name': 'assemble-1',
'publisher_port': 1666,
'server_port': 1667,
'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml',
}
])
move_gripper_1 = Node(
"""gz_get_entity_state = Node(
package='plansys2_bt_actions',
executable='bt_action_node',
name='move_gripper',
namespace=namespace,
output='screen',
parameters=[
pkg_dir + '/config/params.yaml',
robot_description,
robot_description_semantic,
kinematics_yaml,
{
'action_name': 'move_gripper',
'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/move_gripper.xml',
}
])
print_1 = Node(
package='plansys2_bt_actions',
executable='bt_action_node',
name='print',
name='get_part_state',
namespace=namespace,
output='screen',
parameters=[
pkg_dir + '/config/params.yaml',
{
'action_name': 'print',
'bt_xml_file': pkg_dir + '/behavior_trees_xml/test_tree.xml',
'action_name':'get_part_state',
'bt_xml_file':pkg_dir + '/behavior_trees_xml/atomic_skills_xml/get_part_state.xml'
}
])
# transport_1 = Node(
# package='robossembler',
# executable='move_action_node',
# name='transport_1',
# namespace=namespace,
# output='screen',
# parameters=[])
]
)"""
ld = LaunchDescription()
ld.add_action(declare_namespace_cmd)
if __debug == False:
ld.add_action(declare_robot_description)
#ld.add_action(declare_robot_description)
#ld.add_action(gz_get_entity_state)
# Declare the launch options
ld.add_action(plansys2_cmd)
ld.add_action(move_1)
ld.add_action(move_gripper_1)
ld.add_action(print_1)
# ld.add_action(transport_1)
ld.add_action(assemble_1)
return ld

3
robossembler/package.xml
View file

@ -28,7 +28,8 @@
<depend>plansys2_pddl_parser</depend>
<depend>plansys2_bt_actions</depend>
<depend>ament_index_cpp</depend>
<depend>behavior_tree</depend>
<depend>robossembler_interfaces</depend>
<depend>behavior_tree</depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>

32
robossembler/pddl/domain.pddl
View file

@ -6,6 +6,8 @@
robot
zone
gap
part
gripper
frame
printer
material
@ -13,13 +15,7 @@
;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;;
(:predicates
(robot_move ?r - robot ?z - zone)
(gripper_move ?r - robot ?g - gap)
(frame_print ?f - frame ?p - printer ?m - material)
(enviroment_setup ?r - robot ?g - gripper ?p - part)
);; end Predicates ;;;;;;;;;;;;;;;;;;;;
;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;;
@ -28,22 +24,16 @@
);; end Functions ;;;;;;;;;;;;;;;;;;;;
;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(:durative-action move
:parameters (?r - robot ?z - zone)
(:durative-action assemble-1
:parameters (?r - robot ?g - gripper ?p - part)
:duration ( = ?duration 1)
:effect (and (at end(robot_move ?r ?z)))
:effect (and(at start (enviroment_setup ?r ?g ?p)))
)
(:durative-action move_gripper
:parameters (?r - robot ?g - gap)
:duration ( = ?duration 1)
:effect (and (at end(gripper_move ?r ?g)))
)
(:durative-action print
:parameters (?f - frame ?p - printer ?m - material)
:duration (= ?duration 1)
:effect (and (at end(frame_print ?f ?p ?m)))
)
;(:durative-action move_gripper
; :parameters (?r - robot ?g - gap)
; :duration ( = ?duration 1)
; :effect (at end(gripper_move ?r ?g))
;)
);; end Domain ;;;;;;;;;;;;;;;;;;;;;;;;

22
robossembler/src/behavior_tree_nodes/Print.cpp
View file

@ -18,21 +18,23 @@ Print::Print(const std::string &xml_tag_name,
a.position.y = 0.8;
a.position.z = 0.0;
geometry_msgs::msg::Pose b;
a.position.x = -0.8;
a.position.y = -0.8;
a.position.z = 0.0;
printer_coords_.insert(std::make_pair("printer_b", b));
printer_coords_.insert(std::make_pair("printer_a", a));
a.position.x = -0.5;
a.position.y = -0.5;
a.position.z = 0.05;
printer_coords_.insert(std::make_pair("printerB", b));
printer_coords_.insert(std::make_pair("printerA", a));
}
SpawnEntitySrv::Request::SharedPtr Print::populate_request()
{
std::string frame, printer, material;
getInput<std::string>("frame", frame_);
getInput<std::string>("printer", printer_);
getInput<std::string>("material", material_);
std::string package_share_directory = ament_index_cpp::get_package_share_directory("robossembler_sdf_models");
printer_ = "printerA";
material_="material1";
std::string package_share_directory = ament_index_cpp::get_package_share_directory("sdf_models");
std::filesystem::path sdf_path = package_share_directory + "/models/" + frame_ + "/model.sdf";
RCLCPP_INFO(_node->get_logger(), "Start read frame (%s) frame_path (%s)", frame_.c_str(), sdf_path.c_str());
@ -46,8 +48,7 @@ SpawnEntitySrv::Request::SharedPtr Print::populate_request()
} else {
RCLCPP_WARN(_node->get_logger(), "Failed while try to open file (%s)", sdf_path.c_str());
// TODO:
// - exit from service
return 0;
}
std::string xml = buffer.str();
@ -56,7 +57,6 @@ SpawnEntitySrv::Request::SharedPtr Print::populate_request()
auto request = std::make_shared<SpawnEntitySrv::Request>();
request->name = frame_;
request->robot_namespace = "";
request->initial_pose = pose;
request->xml = xml;
RCLCPP_INFO(_node->get_logger(), "Connecting to '/spawn_entity' service...");

15
robossembler/src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp
View file

@ -1,15 +0,0 @@
#include <iostream>
#include <rclcpp/executors/single_threaded_executor.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/qos.hpp>
#include <string>
#include <thread>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
//#include "rasms_bt/behavior_tree_nodes/EmuPoseEstimation.hpp"
int main(int argc, char * argv[])
{
return 0;
}

47
robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp Normal file
View file

@ -0,0 +1,47 @@
#include "gazebo_msgs/srv/get_entity_state.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtService.hpp"
using namespace BT;
using GetEntityStateSrv = gazebo_msgs::srv::GetEntityState;
class GetEntityState : public BtService<GetEntityStateSrv>
{
public:
GetEntityState(const std::string & name, const BT::NodeConfiguration & config)
: BtService<GetEntityStateSrv>(name, config){
}
GetEntityStateSrv::Request::SharedPtr populate_request() override
{
getInput<std::string>("part_name", part_name_);
//part_name_ = getInput<std::string>("PartName").value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< part_name_<<"]");
//GetEntityStateSrv::Request::SharedPtr request;
auto request = std::make_shared<GetEntityStateSrv::Request>();
request->name = part_name_;
RCLCPP_INFO(_node->get_logger(), "Request populated");
return request;
}
BT::NodeStatus handle_response(GetEntityStateSrv::Response::SharedPtr response) override
{
RCLCPP_INFO(_node->get_logger(), "Service call complete");
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("part_name")});
}
private:
std::string part_name_;
};
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<GetEntityState>("GetEntityState");
}

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

@ -1,179 +0,0 @@
#include <iostream>
#include <thread>
#include "rclcpp/executors/single_threaded_executor.hpp"
//#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp/qos.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_scene_interface/planning_scene_interface.h"
#include "robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp"
namespace task_planner
{
static const rclcpp::Logger LOGGER = rclcpp::get_logger("task_planner_move_bt_node");
Move::Move(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf)
: ActionNodeBase(xml_tag_name, conf)
{
rclcpp::Node::SharedPtr node;
config().blackboard->get("node", node);
try
{
node->declare_parameter("arm_group");
node->declare_parameter("waypoints");
node->declare_parameter("waypoint_coords");
}
catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex)
{
RCLCPP_WARN(LOGGER, "%c", ex.what());
}
if (node->has_parameter("arm_group"))
{
node->get_parameter_or("arm_group", available_groups_, {});
}else{
RCLCPP_WARN(LOGGER, "No arm_group names provided");
}
if (node->has_parameter("waypoints"))
{
std::vector<std::string> 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 & e){
}
std::vector<double> 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];
if (coords.size() < 4)
{
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
pose.orientation.z = 0.0;
pose.orientation.w = 1.0;
} else {
pose.orientation.x = coords[3];
pose.orientation.y = coords[4];
pose.orientation.z = coords[5];
pose.orientation.w = coords[6];
}
waypoints_[wp] = pose;
} else {
RCLCPP_WARN(LOGGER, "No coordinate configured for waypoint [%s]", wp.c_str());
}
}
}
}
void Move::halt()
{
_halt_requested = true;
RCLCPP_INFO(LOGGER, "Move halt");
}
BT::NodeStatus Move::tick()
{
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto move_group_node = rclcpp::Node::make_shared("robossembler_move_group_interface", 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);
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 (%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());
}
//Setup MoveIt interface for planning
static const std::string PLANNING_GROUP = 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);
geometry_msgs::msg::Pose goal_pos;
goal_pos.orientation.x = pose2moveit.orientation.x;
goal_pos.orientation.y = pose2moveit.orientation.y;
goal_pos.orientation.z = pose2moveit.orientation.z;
goal_pos.orientation.w = pose2moveit.orientation.w;
goal_pos.position.x = pose2moveit.position.x;
goal_pos.position.y = pose2moveit.position.y;
goal_pos.position.z = pose2moveit.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;
bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if (success)
{
RCLCPP_INFO(LOGGER, "Planning success");
move_group_interface.execute(my_plan);
move_group_interface.move();
} else {
RCLCPP_WARN(LOGGER, "Failed to generate a plan");
return BT::NodeStatus::FAILURE;
}
return BT::NodeStatus::SUCCESS;
}
} // namespace atomic_skills
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<task_planner::Move>("Move");
}

196
robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp
View file

@ -1,152 +1,50 @@
#include <iostream>
#include <thread>
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp/qos.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_scene_interface/planning_scene_interface.h"
#include "robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp"
namespace task_planner
{
static const rclcpp::Logger LOGGER = rclcpp::get_logger("task_planner_move_gripper_bt_node");
MoveGripper::MoveGripper(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf)
: ActionNodeBase(xml_tag_name, conf)
{
rclcpp::Node::SharedPtr node;
config().blackboard->get("node", node);
try
{
node->declare_parameter("gripper_group");
node->declare_parameter("relative_gaps");
node->declare_parameter("gaps");
}
catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex)
{
RCLCPP_WARN(LOGGER, "%c", ex.what());
}
if (node->has_parameter("gripper_group"))
{
node->get_parameter_or("gripper_group", available_groups_, {});
}else{
RCLCPP_WARN(LOGGER, "No gripper_group names provided");
}
if (node->has_parameter("relative_gaps"))
{
std::vector<std::string> gap_names;
node->get_parameter_or("relative_gaps", gap_names, {});
for (auto &gap: gap_names)
{
try
{
node->declare_parameter("gaps." + gap);
}catch(const rclcpp::exceptions::ParameterAlreadyDeclaredException & e){
}
double gap_value;
if (node->get_parameter_or("gaps." + gap, gap_value, {}))
{
relative_gaps_[gap] = gap_value;
} else {
RCLCPP_WARN(LOGGER, "No gap value configured for relative_gap [%s]", gap.c_str());
}
}
}
}
void MoveGripper::halt()
{
_halt_requested = true;
RCLCPP_INFO(LOGGER, "MoveGripper halt");
}
BT::NodeStatus MoveGripper::tick()
{
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto move_group_gripper_node = rclcpp::Node::make_shared("robossembler_move_group_interface", node_options);
config().blackboard->get("node", move_group_gripper_node);
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_gripper_node);
std::thread([&executor]() { executor.spin(); }).detach();
std::string gripper_group;
getInput<std::string>("gripper_group", gripper_group);
std::string gap;
getInput<std::string>("relative_gap", gap);
if (std::find(available_groups_.begin(), available_groups_.end(), gripper_group) != available_groups_.end())
{
gripper_group_ = gripper_group;
RCLCPP_INFO(LOGGER, "Provided available move_group [%s]", gripper_group.c_str());
}else{
RCLCPP_WARN(LOGGER, "Provided not allowed move_group name [%s]", gripper_group.c_str());
}
double gap2moveit;
if (relative_gaps_.find(gap) != relative_gaps_.end())
{
gap2moveit = relative_gaps_[gap];
RCLCPP_INFO(LOGGER, "Read gap (%s) value:[%f]", gap, gap2moveit);
} else {
RCLCPP_WARN(LOGGER, "No value for relative_gap [%s]", gap.c_str());
}
static const std::string PLANNING_GROUP = gripper_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_gripper_node, move_group_options);
auto gripper_joint_values = move_group_interface.getCurrentJointValues();
moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
move_group_interface.setStartState(start_state);
for (auto &joint_value : gripper_joint_values) {
joint_value = gap2moveit > 0? std::min(gap2moveit, max_gripper_gap_): 0.0;
}
move_group_interface.setJointValueTarget(gripper_joint_values);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if (success)
{
RCLCPP_INFO(LOGGER, "Planning success");
move_group_interface.execute(my_plan);
move_group_interface.move();
} else {
RCLCPP_WARN(LOGGER, "Failed to generate a plan");
return BT::NodeStatus::FAILURE;
}
return BT::NodeStatus::SUCCESS;
}
} // namespace atomic_skills
#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
using namespace BT;
using MoveToJointStateAction = robossembler_interfaces::action::MoveitSendJointStates;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_to_joitn_state_action_client");
class MoveToJointState : public BtAction<MoveToJointStateAction>
{
public:
MoveToJointState(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToJointStateAction>(name, config) {
gripper_gap = 0.02;
}
MoveToJointStateAction::Goal populate_goal() override
{
getInput<std::string>("arm_group", robot_name_);
//robot_name_ = getInput<std::string>("arm_group").value();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
auto goal = MoveToJointStateAction::Goal();
goal.robot_name = robot_name_;
goal.joints_acceleration_scaling_factor = 0.1;
goal.joints_velocity_scaling_factor = 0.1;
goal.joint_value = gripper_gap;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
}
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("arm_group")});
}
private:
double gripper_gap{0};
std::string robot_name_;
}; // class MoveToJointState
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<task_planner::MoveGripper>("MoveGripper");
}
factory.registerNodeType<MoveToJointState>("MoveGripper");
}

52
robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp Normal file
View file

@ -0,0 +1,52 @@
#include "robossembler_interfaces/action/moveit_send_pose.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
using namespace BT;
using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPointActionClient");
class MoveToPose : public BtAction<MoveToPoseAction>
{
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config) {
target_pose_.position.x = 0.1;
target_pose_.position.y = 0.1;
target_pose_.position.z = 0.6;
}
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("arm_group", robot_name_);
//robot_name_ = getInput<std::string>("arm_group").value();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
auto goal = MoveToPoseAction::Goal();
goal.robot_name = robot_name_;
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
goal.target_pose = target_pose_;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
}
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("arm_group")});
}
private:
geometry_msgs::msg::Pose target_pose_;
std::string robot_name_;
}; // class MoveToPose
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToPose>("MoveToPose");
}

70
robossembler/src/move_action_node.cpp
View file

@ -1,70 +0,0 @@
// 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 <memory>
#include <algorithm>
#include "plansys2_executor/ActionExecutorClient.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp"
using namespace std::chrono_literals;
class MoveAction : public plansys2::ActionExecutorClient
{
public:
MoveAction()
: plansys2::ActionExecutorClient("move", 500ms)
{
progress_ = 0.0;
}
private:
void do_work()
{
if (progress_ < 1.0) {
progress_ += 0.05;
send_feedback(progress_, "Move running");
} else {
finish(true, 1.0, "Move completed");
progress_ = 0.0;
std::cout << std::endl;
}
std::cout << "\r\e[K" << std::flush;
std::cout << "Moving ... [" << 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<MoveAction>();
node->set_parameter(rclcpp::Parameter("action_name", "move"));
node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
rclcpp::spin(node->get_node_base_interface());
rclcpp::shutdown();
return 0;
}

87
robossembler/src/move_controller_node.cpp
View file

@ -25,23 +25,44 @@ public:
{
}
void init()
bool 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>();
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{"rasmt_arm_group", "robot"});
problem_expert_->addInstance(plansys2::Instance{"one", "zone"});
problem_expert_->addInstance(plansys2::Instance{"cube", "part"});
problem_expert_->addInstance(plansys2::Instance("rasmt_hand_arm_group", "gripper"));
problem_expert_->setGoal(plansys2::Goal("(and(enviroment_setup rasmt_arm_group rasmt_hand_arm_group cube))"));
}
std::string status_to_string(int8_t status)
/*std::string status_to_string(int8_t status)
{
switch (status)
{
@ -64,41 +85,20 @@ public:
return "UNKNOWN";
break;
}
}
}*/
void step()
{
problem_expert_->setGoal(plansys2::Goal("(and(robot_move rasmt_arm_group one))"));
{
if (!executor_client_->execute_and_check_plan()) { // Plan finished
auto result = executor_client_->getResult();
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;
}
executor_client_->start_plan_execution(plan.value());
auto feedback = executor_client_->getFeedBack();
for (const auto & action_feedback : feedback.action_execution_status)
{
std::cout << "[" << action_feedback.action << " " <<
action_feedback.completion * 100.0 << "%]";
}
std::cout << std::endl;
if (!executor_client_->execute_and_check_plan() && executor_client_->getResult())
{
if (executor_client_->getResult().value().success)
{
std::cout << "Successful finished " << std::endl;
}
if (result.value().success) {
RCLCPP_INFO(get_logger(), "Plan succesfully finished");
} else {
RCLCPP_ERROR(get_logger(), "Plan finished with error");
}
}
}
private:
std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
@ -112,16 +112,21 @@ int main(int argc, char ** argv)
rclcpp::init(argc, argv);
auto node = std::make_shared<MoveController>();
node->init();
rclcpp::Rate rate(5);
node->step();
//node->init();
if (!node->init())
{
RCLCPP_ERROR(node->get_logger(), "Error in Init");
return 0;
}
rate.sleep();
rclcpp::spin_some(node->get_node_base_interface());
node->step();
rclcpp::Rate rate(5);
while (rclcpp::ok()) {
node->step();
rate.sleep();
rclcpp::spin_some(node->get_node_base_interface());
}
rate.sleep();
rclcpp::shutdown();
return 0;

37
robossembler_interfaces/CMakeLists.txt Normal file
View file

@ -0,0 +1,37 @@
cmake_minimum_required(VERSION 3.5)
project(robossembler_interfaces)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
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(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/MoveitSendPose.action"
"action/MoveitSendJointStates.action"
"msg/PropertyValuePair.msg"
"msg/ActionFeedbackStatusConstants.msg"
"msg/ActionResultStatusConstants.msg"
DEPENDENCIES geometry_msgs
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

19
robossembler_interfaces/action/MoveitSendJointStates.action Normal file
View file

@ -0,0 +1,19 @@
##Description: Moves robot arm to a specified joint space.
#goal definition
string robot_name
robossembler_interfaces/PropertyValuePair[] joint_pairs # list of joint_pairs (joint name + value)
float32 joint_value # send joint value to gripper
float32 joints_velocity_scaling_factor
float32 joints_acceleration_scaling_factor
float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.
---
#result definition
bool success
uint64 millis_passed
string status #Use the constants of ActionResultStatusConstants in the status field
---
#feedback
bool success
uint64 millis_passed
string status #Use the constants of ActionFeedbackStatusConstants in the status field

24
robossembler_interfaces/action/MoveitSendPose.action Normal file
View file

@ -0,0 +1,24 @@
##Description: Moves robot arm to a specified pose relative to the frame in header.frame_id of target_pose
#goal definition
#Used to indicate which hardcoded motion constraint to use
#e.g 0 no constraint, 1 keep the same end effector orientation
int32 constraint_mode
#similar to geometry_msgs/PoseStamped but using euler instead of quaternion
#at target_pose->header.frame_id define the tf frame
# which the pose will be calculated relative from
geometry_msgs/Pose target_pose
string robot_name
float32 end_effector_velocity
float32 end_effector_acceleration
float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.
---
#result definition
bool success
uint64 millis_passed
string status #Use the constants of ActionResultStatusConstants in the status field
---
#feedback
bool success
uint64 millis_passed
string status #Use the constants of ActionFeedbackStatusConstants in the status field

5
robossembler_interfaces/msg/ActionFeedbackStatusConstants.msg Normal file
View file

@ -0,0 +1,5 @@
string PLANNING="Planning"
string EXECUTING="Executing"
string EMERGENCY_STOP="EmergencyStop"
string OPERATIONAL_EXCEPTION="OperationalException"
string CANCELLING="Cancelling"

6
robossembler_interfaces/msg/ActionResultStatusConstants.msg Normal file
View file

@ -0,0 +1,6 @@
string SUCCESS="Success"
string PLANNING_FAILED="PlanningFailed"
string CONTROL_FAILED="ControlFailed"
string EMERGENCY_STOP="EmergencyStop"
string OPERATIONAL_EXCEPTION="OperationalException"
string CANCELLED="Cancelled"

5
robossembler_interfaces/msg/PropertyValuePair.msg Normal file
View file

@ -0,0 +1,5 @@
##Usage: In MoveToJointsMoveIt.action
##Definition: A property name - Value definition.
string name
float64 value

23
robossembler_interfaces/package.xml Normal file
View file

@ -0,0 +1,23 @@
<?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>robossembler_interfaces</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

70
robossembler_servers/CMakeLists.txt Normal file
View file

@ -0,0 +1,70 @@
cmake_minimum_required(VERSION 3.5)
project(robossembler_servers)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
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(rclcpp_components REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(robossembler_interfaces REQUIRED)
set(deps
rclcpp
rclcpp_action
moveit_core
moveit_ros_planning
moveit_ros_planning_interface
moveit_msgs
geometry_msgs
tf2_ros
rclcpp_components
robossembler_interfaces
)
#
# include_directories(include)
# install(DIRECTORY include
# DESTINATION include
# )
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
# target_include_directories(move_to_joint_states_action_server PRIVATE include)
ament_target_dependencies(move_to_joint_states_action_server ${deps})
add_executable(move_topose_action_server src/move_topose_action_server.cpp)
# target_include_directories(move_topose_action_server PRIVATE include)
ament_target_dependencies(move_topose_action_server ${deps})
install(
TARGETS
move_topose_action_server
move_to_joint_states_action_server
DESTINATION lib/${PROJECT_NAME}
)
ament_package()

29
robossembler_servers/package.xml Normal file
View file

@ -0,0 +1,29 @@
<?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>robossembler_servers</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_msgs</depend>
<depend>tf2_ros</depend>
<depend>rclcpp_action</depend>
<depend>geometry_msgs</depend>
<depend>action_msgs</depend>
<depend>robossembler_interfaces</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

167
robossembler_servers/src/move_to_joint_states_action_server.cpp Normal file
View file

@ -0,0 +1,167 @@
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"
// action libs
#include "rclcpp_action/rclcpp_action.hpp"
#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform.hpp"
// moveit libs
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
/*
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
//#include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp"
namespace robossembler_actions
{
class MoveToJointStateActionServer : public rclcpp::Node
{
public:
using MoveitSendJointStates = robossembler_interfaces::action::MoveitSendJointStates;
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_action_server"), node_(node)
{
// using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(),
// this->get_node_clock_interface(),
// this->get_node_logging_interface(),
// this->get_node_waitables_interface(),
// "move_topose",
// std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
// std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
}
void init()
{
action_server_ = rclcpp_action::create_server<MoveitSendJointStates>(
node_->get_node_base_interface(),
node_->get_node_clock_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(),
"move_to_joint_states",
std::bind(&MoveToJointStateActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveToJointStateActionServer::cancel_callback, this, std::placeholders::_1),
std::bind(&MoveToJointStateActionServer::accepted_callback, this, std::placeholders::_1)
);
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendJointStates>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
rclcpp_action::GoalResponse goal_callback(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveitSendJointStates::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", goal->robot_name.c_str());
(void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received cancel request");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
using namespace std::placeholders;
std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), goal_handle).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendJointStates::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
const moveit::core::JointModelGroup* joint_model_group =
move_group_interface.getCurrentState()->getJointModelGroup(goal->robot_name);
moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState(10);
std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
joint_group_positions[0] = goal->joint_value;
joint_group_positions[1] = goal->joint_value;
move_group_interface.setJointValueTarget(joint_group_positions);
move_group_interface.setMaxVelocityScalingFactor(goal->joints_velocity_scaling_factor);
move_group_interface.setMaxAccelerationScalingFactor(goal->joints_acceleration_scaling_factor);
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if(success)
{
RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(plan);
move_group_interface.move();
}else{
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
}
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveToJointStateActionServer
}// namespace robossembler_actions
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
robossembler_actions::MoveToJointStateActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
return 0;
}

161
robossembler_servers/src/move_topose_action_server.cpp Normal file
View file

@ -0,0 +1,161 @@
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"
// action libs
#include "rclcpp_action/rclcpp_action.hpp"
#include "robossembler_interfaces/msg/action_feedback_status_constants.hpp"
#include "robossembler_interfaces/action/moveit_send_pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform.hpp"
// moveit libs
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
/*
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
//#include <eigen_conversions/eigen_msg.h>
#include "moveit_msgs/msg/display_robot_state.hpp"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "moveit_msgs/msg/robot_trajectory.hpp"
#include "moveit_msgs/action/move_group.hpp"
namespace robossembler_actions
{
class MoveToPoseActionServer : public rclcpp::Node
{
public:
using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_action_server"), node_(node)
{
// using namespace std::placeholders;
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
// this->get_node_base_interface(),
// this->get_node_clock_interface(),
// this->get_node_logging_interface(),
// this->get_node_waitables_interface(),
// "move_topose",
// std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
// std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
}
void init()
{
action_server_ = rclcpp_action::create_server<MoveitSendPose>(
node_->get_node_base_interface(),
node_->get_node_clock_interface(),
node_->get_node_logging_interface(),
node_->get_node_waitables_interface(),
"move_topose",
std::bind(&MoveToPoseActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveToPoseActionServer::cancel_callback, this, std::placeholders::_1),
std::bind(&MoveToPoseActionServer::accepted_callback, this, std::placeholders::_1)
);
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
rclcpp_action::GoalResponse goal_callback(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveitSendPose::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]",
goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z,
goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z,
goal->target_pose.orientation.w);
(void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received cancel request");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
using namespace std::placeholders;
std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), goal_handle).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
move_group_interface.setPoseTarget(goal->target_pose);
move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if(success)
{
RCLCPP_INFO(this->get_logger(), "Planning success");
move_group_interface.execute(plan);
move_group_interface.move();
}else{
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
}
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveToPoseActionServer
}// namespace robossembler_actions
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
robossembler_actions::MoveToPoseActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::spin(node);
run_server.join();
return 0;
}

34
sdf_models/CMakeLists.txt Normal file
View file

@ -0,0 +1,34 @@
cmake_minimum_required(VERSION 3.5)
project(sdf_models)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
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)
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()
install(DIRECTORY models DESTINATION share/${PROJECT_NAME})
ament_package()

14
sdf_models/models/cube/model.config Normal file
View file

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<model>
<name>cube</name>
<sdf version="1.4">model.sdf</sdf>
<author>
<name>Splinter1984</name>
<email>rom.andrianov1984@gmail.com</email>
</author>
<description>
A simple cube.
</description>
</model>

28
sdf_models/models/cube/model.sdf Normal file
View file

@ -0,0 +1,28 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="cube">
<pose>0 0 0 0 0 0</pose>
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
</link>
</model>
</sdf>

18
sdf_models/package.xml Normal file
View file

@ -0,0 +1,18 @@
<?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>sdf_models</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="rom.andrianov1984@gmail.com">Splinter1984</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>