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

test scene for pick and place

Browse source
This commit is contained in:
Ilya Uraev 2022-03-12 13:29:26 +04:00
parent b4339edc19
commit 91f159e289
18 changed files with 219 additions and 133 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -174,29 +174,29 @@ def generate_launch_description():
]
)
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
]
)
# 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)
# launch_nodes.append(move_to_joint_state_action_server)

9
rasmt_support/config/rasmt_ros2_controllers.yaml
View file

@ -6,7 +6,7 @@ controller_manager:
type: joint_trajectory_controller/JointTrajectoryController
rasmt_hand_controller:
type: effort_controllers/JointGroupEffortController
type: forward_command_controller/ForwardCommandController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
@ -32,9 +32,4 @@ rasmt_hand_controller:
joints:
- rasmt_Slide_1
- rasmt_Slide_2
command_interfaces:
- effort
state_interfaces:
- position
- velocity
- effort
interface_name: position

7
rasmt_support/launch/rasmt_control.launch.py
View file

@ -67,6 +67,10 @@ def generate_launch_description():
executable="spawner.py",
arguments=["rasmt_hand_controller", "--controller-manager", "/controller_manager"],
)
action_server_controller_hand_node = Node(
package="robossembler_servers",
executable="gripper_cmd_node"
)
return LaunchDescription(
launch_args + [
@ -74,5 +78,6 @@ def generate_launch_description():
robot_state_publisher,
joint_state_broadcaster,
controller_arm,
controller_hand
controller_hand,
action_server_controller_hand_node
])

8
rasmt_support/urdf/tools/rasmt_hand_control.xacro
View file

@ -17,9 +17,9 @@
</xacro:unless>
<joint name="${prefix}_Slide_1">
<command_interface name="effort">
<command_interface name="position">
<!-- better to use radians as min max first -->
<param name="min">-10</param>
<param name="min">0</param>
<param name="max">10</param>
</command_interface>
<state_interface name="position"/>
@ -30,8 +30,8 @@
<joint name="${prefix}_Slide_2">
<!-- <param name="mimic">${prefix}_Slide_1</param>
<param name="multiplier">1</param> -->
<command_interface name="effort">
<param name="min">-10</param>
<command_interface name="position">
<param name="min">0</param>
<param name="max">10</param>
</command_interface>
<state_interface name="position"/>

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

@ -52,7 +52,7 @@
type="fixed">
<origin
xyz="0 0.0 0.12324"
rpy="-3.14159265358979 0 -1.5707963267949" />
rpy="-3.14159265358979 0 0" />
<parent
link="${prefix}_Dock_Link" />
<child

2
robossembler/CMakeLists.txt
View file

@ -21,6 +21,7 @@ find_package(plansys2_bt_actions REQUIRED)
find_package(gazebo_msgs REQUIRED)
find_package(robossembler_interfaces REQUIRED)
find_package(behavior_tree REQUIRED)
find_package(control_msgs REQUIRED)
if (NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
@ -46,6 +47,7 @@ set(dependencies
robossembler_interfaces
behavior_tree
std_msgs
control_msgs
)
include_directories(include)

9
robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml
View file

@ -5,8 +5,11 @@
<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" />
<Action ID="MoveToPose" command="go_to" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd" command="open" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" command="pick" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd" command="close" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" command="go_to" arm_group="${arg0}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
@ -16,9 +19,11 @@
</Action>
<Action ID="MoveToPose">
<input_port name="arm_group"/>
<input_port name="command"/>
</Action>
<Action ID="MoveGripper">
<input_port name="arm_group"/>
<input_port name="command"/>
</Action>
<Action ID="Print">
<input_port name="frame"/>

7
robossembler/config/params.yaml
View file

@ -15,3 +15,10 @@ assemble_1:
printer2: [-1.0, -1.0, 0.0,
0.0, 0.0, 0.0,
0.0]
pick_entity:
go_to: [0.40644, 0.0, 0.3274, 0.0, 1.0, 0.0, 0.009]
bring: [0.40644, 0.0, 0.2274, 0.0, 1.0, 0.0, 0.009]
gripp_cmd:
open: 0.06
close: 0.02

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

@ -9,17 +9,16 @@
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/rclcpp.hpp>
using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
class Print : public BtService<SpawnEntitySrv>
class Print : public BtService<gazebo_msgs::srv::SpawnEntity>
{
public:
Print(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf);
SpawnEntitySrv::Request::SharedPtr populate_request();
gazebo_msgs::srv::SpawnEntity::Request::SharedPtr populate_request();
BT::NodeStatus handle_response(SpawnEntitySrv::Response::SharedPtr response);
BT::NodeStatus handle_response(gazebo_msgs::srv::SpawnEntity::Response::SharedPtr response);
static BT::PortsList providedPorts()
{

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

@ -9,6 +9,8 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include "robot_bt/behavior_tree_nodes/Print.hpp"
using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
Print::Print(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf)
:BtService<SpawnEntitySrv>(xml_tag_name, conf)
@ -18,8 +20,8 @@ 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.5;
a.position.y = -0.5;
a.position.x = 0.4;
a.position.y = 0.0;
a.position.z = 0.05;
printer_coords_.insert(std::make_pair("printerB", b));
printer_coords_.insert(std::make_pair("printerA", a));

2
robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp
View file

@ -28,7 +28,7 @@ class GetEntityState : public BtService<GetEntityStateSrv>
}
BT::NodeStatus handle_response(GetEntityStateSrv::Response::SharedPtr response) override
{
RCLCPP_INFO(_node->get_logger(), "Service call complete");
RCLCPP_INFO_STREAM(_node->get_logger(), "part_name ["<< part_name_ <<"] Entity state x = "<< response->state.pose.position.x <<" y = "<< response->state.pose.position.y <<" z = "<< response->state.pose.position.z <<"");
return BT::NodeStatus::SUCCESS;
}

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

@ -1,33 +1,34 @@
#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
//#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
//#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "robossembler_interfaces/action/gripper_command.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");
using GripperCmd = robossembler_interfaces::action::GripperCommand;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_cmd");
class MoveToJointState : public BtAction<MoveToJointStateAction>
class SendGripperCmd : public BtAction<GripperCmd>
{
public:
MoveToJointState(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToJointStateAction>(name, config) {
gripper_gap = 0.02;
SendGripperCmd(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<GripperCmd>(name, config) {
//gripper_gap = 0.02;
gripper_point_a_ = 0.06;
gripper_point_b_ = 0.025;
gripper_cmd.insert(std::make_pair("open", gripper_point_a_));
gripper_cmd.insert(std::make_pair("close", gripper_point_b_));
}
MoveToJointStateAction::Goal populate_goal() override
GripperCmd::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");
auto goal = GripperCmd::Goal();
getInput<std::string>("command", command);
goal.value = gripper_cmd[command];
//goal.trajectory.points = gripper_cmd[command];
return goal;
}
@ -35,16 +36,19 @@ class MoveToJointState : public BtAction<MoveToJointStateAction>
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("arm_group")});
InputPort<std::string>("command")});
}
private:
double gripper_gap{0};
std::string robot_name_;
std::vector<std::string> joint_names_ = {"rasmt_Slide_1", "rasmt_Slide_2"};
double gripper_point_a_{0.0};
double gripper_point_b_{0.0};
std::map<std::string, double> gripper_cmd;
std::string command;
}; // class MoveToJointState
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToJointState>("MoveGripper");
factory.registerNodeType<SendGripperCmd>("GripperCmd");
}

42
robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
View file

@ -3,6 +3,8 @@
#include "behavior_tree/BtAction.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
#include "Eigen/Geometry"
#include "rclcpp/parameter.hpp"
using namespace BT;
using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose;
@ -13,26 +15,39 @@ class MoveToPose : public BtAction<MoveToPoseAction>
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config) {
target_pose_.position.x = 0.2;
target_pose_.position.y = 0.2;
target_pose_.position.z = 0.7;
//target_pose_.orientation.x = 0;
//target_pose_.orientation.y = 0.9764;
//target_pose_.orientation.z = 0;
//target_pose_.orientation.w = 0.2160;
target_pose_a_.position.x = 0.40644;
target_pose_a_.position.y = 0.0;
target_pose_a_.position.z = 0.3274;
target_pose_a_.orientation.x = 0;
target_pose_a_.orientation.y = 1.0;
target_pose_a_.orientation.z = 0.0;
target_pose_a_.orientation.w = 0.009;
target_pose_b_.position.x = 0.40644;
target_pose_b_.position.y = 0.0;
target_pose_b_.position.z = 0.20;
target_pose_b_.orientation.x = 0;
target_pose_b_.orientation.y = 1.0;
target_pose_b_.orientation.z = 0.0;
target_pose_b_.orientation.w = 0.009;
pick_and_place_command.insert(std::make_pair("go_to", target_pose_a_));
pick_and_place_command.insert(std::make_pair("pick", target_pose_b_));
}
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("arm_group", robot_name_);
//robot_name_ = getInput<std::string>("arm_group").value();
getInput<std::string>("command", command);
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_;
goal.target_pose = pick_and_place_command[command];
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
@ -41,12 +56,17 @@ class MoveToPose : public BtAction<MoveToPoseAction>
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("arm_group")});
InputPort<std::string>("arm_group"),
InputPort<std::string>("command")});
}
private:
geometry_msgs::msg::Pose target_pose_;
geometry_msgs::msg::Pose target_pose_a_;
geometry_msgs::msg::Pose target_pose_b_;
std::string robot_name_;
std::map<std::string, geometry_msgs::msg::Pose> pick_and_place_command;
std::string command;
std::string pick;
}; // class MoveToPose

2
robossembler_interfaces/CMakeLists.txt
View file

@ -23,10 +23,10 @@ find_package(geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/MoveitSendPose.action"
"action/MoveitSendJointStates.action"
"action/GripperCommand.action"
"msg/PropertyValuePair.msg"
"msg/ActionFeedbackStatusConstants.msg"
"msg/ActionResultStatusConstants.msg"
"srv/GripperCommand.srv"
DEPENDENCIES geometry_msgs std_msgs
)

3
robossembler_interfaces/srv/GripperCommand.srv → robossembler_interfaces/action/GripperCommand.action
View file

@ -1,3 +1,4 @@
float64 value
---
string success
bool success
---

19
robossembler_servers/CMakeLists.txt
View file

@ -53,21 +53,18 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
add_library(gripper_cmd_srv_lib SHARED src/gripper_cmd_srv.cpp)
target_compile_definitions(gripper_cmd_srv_lib PRIVATE "GRIPPER_CMD_BUILDING_DLL")
ament_target_dependencies(gripper_cmd_srv_lib ${deps})
rclcpp_components_register_node(gripper_cmd_srv_lib
PLUGIN "GripperCmd"
EXECUTABLE gripper_cmd_srv)
add_library(gripper_cmd_action_server SHARED src/gripper_cmd.cpp)
ament_target_dependencies(gripper_cmd_action_server ${deps})
target_include_directories(gripper_cmd_action_server PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(gripper_cmd_action_server PRIVATE "GRIPPER_CMD_ACTION_SERVER_CPP_BUILDING_DLL")
rclcpp_components_register_node(gripper_cmd_action_server PLUGIN "robossembler_servers::GripperCmd" EXECUTABLE gripper_cmd_node)
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})
@ -75,7 +72,7 @@ install(
TARGETS
move_topose_action_server
move_to_joint_states_action_server
gripper_cmd_srv_lib
gripper_cmd_action_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}

95
robossembler_servers/src/gripper_cmd.cpp Normal file
View file

@ -0,0 +1,95 @@
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "robossembler_interfaces/action/gripper_command.hpp"
using GripperCmdAction = robossembler_interfaces::action::GripperCommand;
namespace robossembler_servers
{
class GripperCmd : public rclcpp::Node
{
public:
explicit GripperCmd(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("gripper_cmd", options)
{
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<GripperCmdAction>(
this,
"/gripper_cmd",
std::bind(&GripperCmd::goal_callback, this, _1, _2),
std::bind(&GripperCmd::cancel_callback, this, _1),
std::bind(&GripperCmd::accepted_callback, this, _1));
this->gripper_cmd_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>
("/rasmt_hand_controller/command", 10);
}
private:
rclcpp_action::Server<GripperCmdAction>::SharedPtr action_server_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_publisher_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCmdAction>;
rclcpp_action::GoalResponse goal_callback(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const GripperCmdAction::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request [%.3f]", goal->value);
(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(&robossembler_servers::GripperCmd::execute, this, _1), goal_handle).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<GripperCmdAction::Feedback>();
auto result = std::make_shared<GripperCmdAction::Result>();
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
return;
}
std_msgs::msg::Float64MultiArray commands;
RCLCPP_INFO(this->get_logger(), "Initialize commands");
commands.data.push_back(goal->value);
commands.data.push_back(goal->value);
//commands.data[0] = goal->value;
//commands.data[1] = goal->value;
RCLCPP_INFO(this->get_logger(), "Publish commands");
this->gripper_cmd_publisher_->publish(commands);
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
}
};
} // namespace robossembler_servers
RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_servers::GripperCmd)

46
robossembler_servers/src/gripper_cmd_srv.cpp
View file

@ -1,46 +0,0 @@
#include <memory>
#include <string>
#include <vector>
#include <cinttypes>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "robossembler_interfaces/srv/gripper_command.hpp"
using GripperCmdSrv = robossembler_interfaces::srv::GripperCommand;
class GripperCmd : public rclcpp::Node
{
public:
explicit GripperCmd(const rclcpp::NodeOptions & options)
: Node("gripper_cmd", options)
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
auto handle_gripper_cmd =
[this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<GripperCmdSrv::Request> request,
std::shared_ptr<GripperCmdSrv::Response> response) -> void
{
RCLCPP_INFO(this->get_logger(), "Service got request value [%f]", request->value);
(void)request_header;
RCLCPP_INFO(this->get_logger(), "request header");
//commands = request->value;
std_msgs::msg::Float64MultiArray msg;
RCLCPP_INFO(this->get_logger(), "init msg");
msg.data[0] = request->value;
RCLCPP_INFO(this->get_logger(), "write to msg");
RCLCPP_INFO(this->get_logger(), "Publishing message");
pub_->publish(msg);
response->success = "SUCCESS";
};
srv_ = create_service<GripperCmdSrv>("/gripper_cmd", handle_gripper_cmd);
pub_ = create_publisher<std_msgs::msg::Float64MultiArray>("/rasmt_hand_controller/commands", 10);
}
private:
rclcpp::Service<GripperCmdSrv>::SharedPtr srv_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr pub_;
}; // class GripperCmd
RCLCPP_COMPONENTS_REGISTER_NODE(GripperCmd)