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

create remove pddl action

Browse source
This commit is contained in:
Splinter1984 2022-04-01 01:36:48 +08:00
parent b5fad5156a
commit 6b7bfd7a71
33 changed files with 470 additions and 296 deletions
Download patch file Download diff file Expand all files Collapse all files

10
rasms.repos
View file

@ -10,4 +10,12 @@ repositories:
plansys2:
type: git
url: https://github.com/IntelligentRoboticsLabs/ros2_planning_system
version: master
version: master
behavior_tree:
type: git
url: https://github.com/berkeleyauv/behavior_tree
version: master
gazebo_ros_link_attacher:
type: git
url: https://github.com/davidorchansky/gazebo_ros_link_attacher.git
version: foxy-devel

22
rasmt_support/world/robossembler.world
View file

@ -69,28 +69,6 @@
<kinematic>0</kinematic>
</link>
</model>
<model name="box">
<pose>0.5 0 0.1 0 0 0</pose>
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>0.15 0.15 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.15 0.15 0.05</size>
</box>
</geometry>
<material>
<script>Gazebo/WoodPallet</script>
</material>
</visual>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>

37
robossembler/behavior_trees_xml/atomic_skills_xml/remove.xml
View file

@ -7,24 +7,53 @@
part_id="${arg0}"
grasp_pose="{grasp_pose}"
pre_grasp_pose="{pre_grasp_pose}"
pre_gap_distance="{pre_gap_distance}"
gap_distance="{gap_distance}"
server_name="/scene_monitor/get_grasp_poses"
server_timeout="10" />
<Action ID="MoveToPose"
arm_group="${arg3}"
robot_name="${arg3}"
pose="{pre_grasp_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="GripperCmd"
gap_distance="{gap_distance}"
gap_distance="{pre_gap_distance}"
frame_name="${arg0}"
server_name="/gripper_cmd"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{grasp_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="GripperCmd"
gap_distance="{gap_distance}"
frame_name="${arg0}"
server_name="/gripper_cmd"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{pre_grasp_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
@ -35,16 +64,18 @@
<output_port name="grasp_pose"/>
<output_port name="pre_grasp_pose"/>
<output_port name="pre_gap_distance"/>
<output_port name="gap_distance"/>
</Action>
<Action ID="MoveToPose">
<input_port name="arm_group"/>
<input_port name="robot_name"/>
<input_port name="pose"/>
</Action>
<Action ID="GripperCmd">
<input_port name="gap_distance"/>
<input_port name="frame_name"/>
</Action>
</TreeNodesModel>

1
robossembler/include/robot_bt/behavior_tree_nodes/GetGraspPoses.hpp
View file

@ -27,6 +27,7 @@ public:
BT::InputPort<std::string>("part_id"),
BT::OutputPort<geometry_msgs::msg::Pose>("grasp_pose"),
BT::OutputPort<geometry_msgs::msg::Pose>("pre_grasp_pose"),
BT::OutputPort<double>("pre_gap_distance"),
BT::OutputPort<double>("gap_distance")
});
}

1
robossembler/src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp
View file

@ -46,6 +46,7 @@ BT::NodeStatus GetGraspPoses::handle_response(GetGraspPosesSrv::Response::Shared
setOutput<geometry_msgs::msg::Pose>("grasp_pose", grasp_pose.grasp_pose);
setOutput<geometry_msgs::msg::Pose>("pre_grasp_pose", grasp_pose.pre_grasp_pose);
setOutput<double>("pre_gap_distance", grasp_pose.gap_distance+0.006);
setOutput<double>("gap_distance", grasp_pose.gap_distance);
return BT::NodeStatus::SUCCESS;

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

@ -12,26 +12,19 @@ class SendGripperCmd : public BtAction<GripperCmd>
{
public:
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.05;
// gripper_cmd.insert(std::make_pair("open", gripper_point_a_));
// gripper_cmd.insert(std::make_pair("close", gripper_point_b_));
: BtAction<GripperCmd>(name, config)
{
}
GripperCmd::Goal populate_goal() override
{
auto goal = GripperCmd::Goal();
// getInput<std::string>("command", command);
//goal.value = gripper_cmd[command];
//goal.trajectory.points = gripper_cmd[command];
getInput<double>("gap_distance", distance_);
getInput<std::string>("frame_name", frame_);
goal.value = distance_;
goal.frame = frame_;
return goal;
}
@ -39,16 +32,15 @@ class SendGripperCmd : public BtAction<GripperCmd>
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<double>("gap_distance")});
InputPort<double>("gap_distance"),
InputPort<std::string>("frame_name")
});
}
private:
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;
double distance_;
std::string frame_;
}; // class MoveToJointState

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

@ -15,53 +15,27 @@ class MoveToPose : public BtAction<MoveToPoseAction>
{
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config) {
// target_pose_a_.position.x = 0.519;//0.40644;
// target_pose_a_.position.y = 0.5;//0.0;
// target_pose_a_.position.z = -0.021;//0.3274;
// target_pose_a_.orientation.x = 0.58;//0;
// target_pose_a_.orientation.y = -0.16;//1.0;
// target_pose_a_.orientation.z = 0.16;//0.0;
// target_pose_a_.orientation.w = 0.77;//0.009;
// target_pose_b_.position.x = 0.0;
// target_pose_b_.position.y = 0.0;
// target_pose_b_.position.z = -0.12;
// target_pose_b_.orientation.x = 0;
// target_pose_b_.orientation.y = 0.0;
// target_pose_b_.orientation.z = 0.0;
// target_pose_b_.orientation.w = 0.0;
// target_pose_c_.position.x = 0.0;
// target_pose_c_.position.y = 0.0;
// target_pose_c_.position.z = 0.12;
// target_pose_c_.orientation.x = 0;
// target_pose_c_.orientation.y = 0.0;
// target_pose_c_.orientation.z = 0.0;
// target_pose_c_.orientation.w = 0.0;
// pick_and_place_command.insert(std::make_pair("go_to", target_pose_a_));
// pick_and_place_command.insert(std::make_pair("down", target_pose_b_));
// pick_and_place_command.insert(std::make_pair("up", target_pose_c_));
: BtAction<MoveToPoseAction>(name, config)
{
}
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("arm_group", robot_name_);
//robot_name_ = getInput<std::string>("arm_group").value();
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::Pose>("pose", pose_);
RCLCPP_INFO(_node->get_logger(), "GrasPose \npose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f",
RCLCPP_INFO(_node->get_logger(), "GrasPose pose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f",
pose_.position.x, pose_.position.y, pose_.position.z,
pose_.orientation.x, pose_.orientation.y, pose_.orientation.z, pose_.orientation.w);
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
goal.robot_name = robot_name_;
goal.target_pose = pose_;
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
goal.target_pose = pose_;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
@ -70,17 +44,14 @@ class MoveToPose : public BtAction<MoveToPoseAction>
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("arm_group"),
InputPort<geometry_msgs::msg::Pose>("pose")});
InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("pose")
});
}
private:
geometry_msgs::msg::Pose target_pose_a_;
geometry_msgs::msg::Pose target_pose_b_, target_pose_c_;
std::string robot_name_;
// std::map<std::string, geometry_msgs::msg::Pose> pick_and_place_command;
geometry_msgs::msg::Pose pose_;
// std::string pick;
}; // class MoveToPose

4
robossembler/src/move_controller_node.cpp
View file

@ -55,7 +55,7 @@ public:
void init_knowledge()
{
problem_expert_->addInstance(plansys2::Instance{"printer1", "printer"});
problem_expert_->addInstance(plansys2::Instance{"output_shaft_211118", "part"});
problem_expert_->addInstance(plansys2::Instance{"box1", "part"});
problem_expert_->addInstance(plansys2::Instance{"rasmt_arm_group", "arm"});
problem_expert_->addInstance(plansys2::Instance{"workspace1", "workspace"});
@ -65,7 +65,7 @@ public:
problem_expert_->addPredicate(plansys2::Predicate{"(printer_ready printer1)"});
problem_expert_->addPredicate(plansys2::Predicate{"(arm_available rasmt_arm_group)"});
problem_expert_->setGoal(plansys2::Goal("(and(part_at output_shaft_211118 workspace1))"));
problem_expert_->setGoal(plansys2::Goal("(and(part_at box1 workspace1))"));
// problem_expert_->setGoal(plansys2::Goal("(and(enviroment_setup rasmt_arm_group rasmt_hand_arm_group cube))"));
}

1
robossembler_interfaces/action/GripperCommand.action
View file

@ -1,4 +1,5 @@
float64 value
string frame
---
bool success
---

18
robossembler_scene_monitor/scene_monitor/config/component_state_node.param.yaml
View file

@ -2,11 +2,19 @@ component_state_monitor_node:
ros__parameters:
models-package: "sdf_models"
global_frame: "world"
printers_names: ["printer1"]
printers_names: ["printer1", "printer2"]
printers:
printer1:
position: [0.5, 0.0, 0.12]
position: [0.3, 0.3, 0.012]
orientation: [0.0, 0.0, 0.0, 1.0]
printer2:
position: [0.0, 0.5, 0.012]
orientation: [0.0, 0.0, 0.0, 0.0]
# printerB:
# position: [2.0, 2.0, 2.0]
# orientation: [2.0, 2.0, 2.0, 2.0]
workspaces_names: ["workspace1", "workspace2"]
workspaces:
workspace1:
position: [-0.5, 0.0, 0.012]
orientation: [0.0, 0.0, 0.0, 0.0]
workspace2:
position: [0.0, -0.5, 0.012]
orientation: [0.0, 0.0, 0.0, 0.0]

0
robossembler_scene_monitor/scene_monitor/include/assembly_state_monitor.hpp
View file

10
robossembler_scene_monitor/scene_monitor/include/component.hpp
View file

@ -29,6 +29,12 @@ enum ComponentState
Posted
};
enum ComponentType
{
Part,
Assembly
};
class Component
{
public:
@ -37,6 +43,8 @@ public:
Component(const std::string &frame_name);
ComponentState getCurrentState() const;
ComponentType getType() const;
std::vector<std::string> getComponentLinks() const;
std::string getFrameName() const;
scene_monitor_interfaces::msg::GraspPose getGraspPose(const std::string &grasp_pose_name) const;
std::vector<scene_monitor_interfaces::msg::GraspPose> getGraspPoses() const;
@ -52,8 +60,10 @@ private:
std::string _frame_name;
ComponentState _current_state;
ComponentType _component_type;
geometry_msgs::msg::Pose _placement;
std::map<std::string, scene_monitor_interfaces::msg::GraspPose> _grasp_poses;
std::vector<std::string> _component_links;
geometry_msgs::msg::Pose _current_pose;
};

7
robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp
View file

@ -14,6 +14,7 @@
#include "scene_monitor_interfaces/srv/get_grasp_pose.hpp"
#include "scene_monitor_interfaces/srv/get_grasp_poses.hpp"
#include "scene_monitor_interfaces/srv/print_part.hpp"
#include "scene_monitor_interfaces/srv/get_workspace_pose.hpp"
#include "component.hpp"
#include "component_spawner.hpp"
@ -26,6 +27,7 @@ using GetPartSrv = scene_monitor_interfaces::srv::GetPart;
using GetGraspPoseSrv = scene_monitor_interfaces::srv::GetGraspPose;
using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses;
using PrintPartSrv = scene_monitor_interfaces::srv::PrintPart;
using GetWorkspacePoseSrv = scene_monitor_interfaces::srv::GetWorkspacePose;
class ComponentStateMonitor: public rclcpp::Node
{
@ -47,11 +49,12 @@ private:
std::shared_ptr<GetGraspPoseSrv::Response> response);
void printComponent(const std::shared_ptr<PrintPartSrv::Request> request,
std::shared_ptr<PrintPartSrv::Response> response);
void getWorkspacePose(const std::shared_ptr<GetWorkspacePoseSrv::Request> request,
std::shared_ptr<GetWorkspacePoseSrv::Response> response);
void makeTransform(const Component& component, const std::string& parent);
void makeTransform(const std::string& frame_name, const geometry_msgs::msg::Pose& frame_pose,
const std::string& parent);
void updateVizMarker(const Component& component);
void updateComponents();
std::map<std::string, Component> _loaded_components;
@ -59,9 +62,11 @@ private:
rclcpp::Service<GetGraspPoseSrv>::SharedPtr _get_grasp_pose_service;
rclcpp::Service<GetGraspPosesSrv>::SharedPtr _get_grasp_poses_service;
rclcpp::Service<PrintPartSrv>::SharedPtr _print_part_service;
rclcpp::Service<GetWorkspacePoseSrv>::SharedPtr _get_workspace_service;
std::string _models_package;
std::map<std::string, geometry_msgs::msg::Pose> _loaded_printers;
std::map<std::string, geometry_msgs::msg::Pose> _loaded_workspaces;
ComponentSpawner _component_spawner_node;
ComponentPoseEstimation _pose_estimation_node;

15
robossembler_scene_monitor/scene_monitor/include/scene_state_monitor.hpp
View file

@ -1,20 +1,13 @@
#include "component_state_monitor.hpp"
#pragma once
#include <rclcpp/rclcpp.hpp>
namespace scene_state_monitor
{
class SceneStateMonitor: public rclcpp::Node
{
public:
SceneStateMonitor();
~SceneStateMonitor();
private:
component_state_monitor::ComponentStateMonitor _component_state_monitor_node;
};
}

1
robossembler_scene_monitor/scene_monitor/package.xml
View file

@ -14,6 +14,7 @@
<depend>tf2_ros</depend>
<depend>std_msgs</depend>
<!-- <depend>visualization_msgs</depend> -->
<depend>gazebo_msgs</depend>
<depend>geometry_msgs</depend>
<!-- <depend>action_msgs</depend> -->
<depend>scene_monitor_interfaces</depend>

25
robossembler_scene_monitor/scene_monitor/src/component.cpp
View file

@ -22,6 +22,16 @@ namespace component_state_monitor
return _frame_name;
}
ComponentType Component::getType() const
{
return _component_type;
}
std::vector<std::string> Component::getComponentLinks() const
{
return _component_links;
}
scene_monitor_interfaces::msg::GraspPose Component::getGraspPose(const std::string &grasp_pose_name) const
{
if (_grasp_poses.find(grasp_pose_name) == _grasp_poses.end())
@ -95,6 +105,18 @@ namespace component_state_monitor
return;
}
std::string type = input.at("type");
_component_type = type == "part"? ComponentType::Part: ComponentType::Assembly;
size_t size = input.at("include").size();
if (size > 0)
{
for (auto it=input.at("include").begin(); it != input.at("include").end(); ++it)
{
_component_links.push_back(it.value());
}
}
_placement = construct_pose(input.at("features").at("placements").at("placement"));
auto grasp_poses = input.at("features").at("grasp-poses");
@ -105,8 +127,7 @@ namespace component_state_monitor
grasp_pose.grasp_pose = construct_pose(it.value().at("placement"));
geometry_msgs::msg::Pose pre_pose;
pre_pose.position.z = 0.12;
//pre_pose.orientation = grasp_pose.grasp_pose.orientation;
pre_pose.position.z -= 0.05;
grasp_pose.pre_grasp_pose = pre_pose;

133
robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp
View file

@ -10,11 +10,12 @@
namespace component_state_monitor
{
using namespace std::chrono_literals;
ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor_node")
ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor")
{
this->declare_parameter<std::string>("models-package", "sdf_models");
this->declare_parameter<std::string>("global_frame", "world");
this->declare_parameter<std::vector<std::string>>("printers_names", std::vector<std::string>{});
this->declare_parameter<std::vector<std::string>>("workspaces_names", std::vector<std::string>{});
RCLCPP_INFO(this->get_logger(), "Initialize [get_part] service");
_get_part_service = this->create_service<GetPartSrv>("scene_monitor/get_part",
@ -35,7 +36,11 @@ namespace component_state_monitor
_print_part_service = this->create_service<PrintPartSrv>("scene_monitor/print_part",
std::bind(&ComponentStateMonitor::printComponent, this,
std::placeholders::_1, std::placeholders::_2));
// RCLCPP_INFO(this->get_logger(), "Initialize [get_workspace_pose] service");
// _get_workspace_service = this->create_service<GetWorkspacePoseSrv>("scene_monitor/get_workspace_pose",
// std::bind(&ComponentStateMonitor::getWorkspacePose, this,
// std::placeholders::_1, std::placeholders::_2));
_tf2_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
@ -57,6 +62,15 @@ namespace component_state_monitor
if (component.second.getCurrentState() == ComponentState::Printed)
{
makeTransform(component.second, _global_frame);
if (component.second.getType() == ComponentType::Assembly)
{
auto links = component.second.getComponentLinks();
for (const auto link: links)
{
auto pose = _pose_estimation_node.framePose(component.first + "::" + link);
makeTransform(link, pose, _global_frame);
}
}
auto grasp_poses = component.second.getGraspPoses();
for (const auto& grasp_pose: grasp_poses)
{
@ -146,11 +160,43 @@ namespace component_state_monitor
tmp_pose.orientation.x = orientation.at(0);
tmp_pose.orientation.y = orientation.at(1);
tmp_pose.orientation.y = orientation.at(2);
tmp_pose.orientation.z = orientation.at(3);
tmp_pose.orientation.z = orientation.at(2);
tmp_pose.orientation.w = orientation.at(3);
_loaded_printers.insert({printer, tmp_pose});
}
std::vector<std::string> workspaces;
this->get_parameter("workspaces_names", workspaces);
for (const auto& workspace: workspaces)
{
std::vector<double> position;
std::vector<double> orientation;
this->declare_parameter<std::vector<double>>("workspaces." + workspace + ".position", std::vector<double>{0.0, 0.0, 0.0});
this->declare_parameter<std::vector<double>>("workspaces." + workspace + ".orientation", std::vector<double>{0.0, 0.0, 0.0, 0.0});
this->get_parameter("workspaces." + workspace + ".position", position);
RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) position (%f, %f, %f)",
workspace.c_str(), position.at(0), position.at(1), position.at(2));
this->get_parameter("workspaces." + workspace + ".orientation", orientation);
RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) orientation (%f, %f, %f, %f)",
workspace.c_str(), orientation.at(0), orientation.at(1), orientation.at(2), orientation.at(3));
geometry_msgs::msg::Pose tmp_pose;
tmp_pose.position.x = position.at(0);
tmp_pose.position.y = position.at(1);
tmp_pose.position.z = position.at(2);
tmp_pose.orientation.x = orientation.at(0);
tmp_pose.orientation.y = orientation.at(1);
tmp_pose.orientation.z = orientation.at(2);
tmp_pose.orientation.w = orientation.at(3);
_loaded_workspaces.insert({workspace, tmp_pose});
}
}
ComponentStateMonitor::~ComponentStateMonitor() {}
@ -179,6 +225,46 @@ namespace component_state_monitor
RCLCPP_INFO(this->get_logger(), "[get_part] Sending back response");
}
void ComponentStateMonitor::getWorkspacePose(const std::shared_ptr<GetWorkspacePoseSrv::Request> request,
std::shared_ptr<GetWorkspacePoseSrv::Response> response)
{
RCLCPP_INFO(this->get_logger(), "[get_workspace_pose] Incoming request");
if (_loaded_workspaces.find(request->workspace_id) != _loaded_workspaces.end())
{
RCLCPP_INFO(this->get_logger(), "[get_workspace_pose] found available workspace (%s) in component_state_monitor", request->workspace_id.c_str());
auto workspace = _loaded_workspaces.at(request->workspace_id);
response->workspace_pose = workspace;
response->success = true;
response->message = "successfully received a response from the service [get_workspace_pose] with workspace (" + request->workspace_id + ")";
} else {
RCLCPP_WARN(this->get_logger(), "[get_workspace_pose] workspaces with name (%s) not available now", request->workspace_id.c_str());
response->success = false;
response->message = "workspace with name (" + request->workspace_id + ") not available";
}
RCLCPP_INFO(this->get_logger(), "[get_workspace_pose] Sending back response");
}
inline geometry_msgs::msg::Pose transformToPose(const geometry_msgs::msg::TransformStamped transformStamped)
{
geometry_msgs::msg::Pose pose;
pose.position.x = transformStamped.transform.translation.x;
pose.position.y = transformStamped.transform.translation.y;
pose.position.z = transformStamped.transform.translation.z;
pose.orientation.x = transformStamped.transform.rotation.x;
pose.orientation.y = transformStamped.transform.rotation.y;
pose.orientation.z = transformStamped.transform.rotation.z;
pose.orientation.w = transformStamped.transform.rotation.w;
return pose;
}
void ComponentStateMonitor::getGraspPoses(const std::shared_ptr<GetGraspPosesSrv::Request> request,
std::shared_ptr<GetGraspPosesSrv::Response> response)
{
@ -195,28 +281,12 @@ namespace component_state_monitor
geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
_global_frame, pose.label, tf2::TimePointZero
);
pose.grasp_pose.position.x = transformStamped.transform.translation.x;
pose.grasp_pose.position.y = transformStamped.transform.translation.y;
pose.grasp_pose.position.z = transformStamped.transform.translation.z;
pose.grasp_pose.orientation.x = transformStamped.transform.rotation.x;
pose.grasp_pose.orientation.y = transformStamped.transform.rotation.y;
pose.grasp_pose.orientation.z = transformStamped.transform.rotation.z;
pose.grasp_pose.orientation.w = transformStamped.transform.rotation.w;
pose.grasp_pose = transformToPose(transformStamped);
geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
_global_frame, std::string("pre_"+pose.label), tf2::TimePointZero
);
pose.pre_grasp_pose.position.x = preTransformStamped.transform.translation.x;
pose.pre_grasp_pose.position.y = preTransformStamped.transform.translation.y;
pose.pre_grasp_pose.position.z = preTransformStamped.transform.translation.z;
pose.pre_grasp_pose.orientation.x = preTransformStamped.transform.rotation.x;
pose.pre_grasp_pose.orientation.y = preTransformStamped.transform.rotation.y;
pose.pre_grasp_pose.orientation.z = preTransformStamped.transform.rotation.z;
pose.pre_grasp_pose.orientation.w = preTransformStamped.transform.rotation.w;
pose.pre_grasp_pose = transformToPose(preTransformStamped);
result_poses.push_back(pose);
}
@ -251,27 +321,13 @@ namespace component_state_monitor
auto grasp_pose = component.getGraspPose(request->grasp_pose_name);
grasp_pose.grasp_pose.position.x = transformStamped.transform.translation.x;
grasp_pose.grasp_pose.position.y = transformStamped.transform.translation.y;
grasp_pose.grasp_pose.position.z = transformStamped.transform.translation.z;
grasp_pose.grasp_pose.orientation.x = transformStamped.transform.rotation.x;
grasp_pose.grasp_pose.orientation.y = transformStamped.transform.rotation.y;
grasp_pose.grasp_pose.orientation.z = transformStamped.transform.rotation.z;
grasp_pose.grasp_pose.orientation.w = transformStamped.transform.rotation.w;
grasp_pose.grasp_pose = transformToPose(transformStamped);
geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
_global_frame, std::string("pre_"+grasp_pose.label), tf2::TimePointZero
);
grasp_pose.pre_grasp_pose.position.x = preTransformStamped.transform.translation.x;
grasp_pose.pre_grasp_pose.position.y = preTransformStamped.transform.translation.y;
grasp_pose.pre_grasp_pose.position.z = preTransformStamped.transform.translation.z;
grasp_pose.pre_grasp_pose.orientation.x = preTransformStamped.transform.rotation.x;
grasp_pose.pre_grasp_pose.orientation.y = preTransformStamped.transform.rotation.y;
grasp_pose.pre_grasp_pose.orientation.z = preTransformStamped.transform.rotation.z;
grasp_pose.pre_grasp_pose.orientation.w = preTransformStamped.transform.rotation.w;
grasp_pose.pre_grasp_pose = transformToPose(preTransformStamped);
response->grasp_pose = grasp_pose;
response->success = true;
@ -300,6 +356,7 @@ namespace component_state_monitor
if (_loaded_components.find(request->frame_id) != _loaded_components.end())
{
auto component = _loaded_components.at(request->frame_id);
if (_loaded_printers.find(request->printer_id) != _loaded_printers.end())
{
auto printer_pose = _loaded_printers.at(request->printer_id);

1
robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt
View file

@ -26,6 +26,7 @@ set(msg_files
)
set(srv_files
"srv/GetWorkspacePose.srv"
"srv/PrintPart.srv"
"srv/GetGraspPose.srv"
"srv/GetGraspPoses.srv"

7
robossembler_scene_monitor/scene_monitor_interfaces/srv/GetWorkspacePose.srv Normal file
View file

@ -0,0 +1,7 @@
string workspace_id
---
#result definition
geometry_msgs/Pose workspace_pose
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages

5
robossembler_servers/src/gripper_cmd.cpp
View file

@ -87,13 +87,14 @@ private:
}
// Compute finger change for detach or attach
auto finger_state_change = goal->value - finger_state;
auto frame_name = goal->frame;
RCLCPP_ERROR(this->get_logger(), "Finger change [%.2f]", finger_state_change);
if(finger_state_change<0.0)
{
send_attach_goal("rasmt", "cube", "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
send_attach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
} else if (finger_state_change>0.0)
{
send_detach_goal("rasmt", "cube", "rasmt_Dock_Link", "link");
send_detach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link");
}
// Send commands to gripper
std_msgs::msg::Float64MultiArray commands;

40
sdf_models/models/box1/frames.json Normal file
View file

@ -0,0 +1,40 @@
{
"label": "box1",
"type": "component",
"include": [],
"features": {
"grasp-poses": {
"Gripper_for_Part__Feature057": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.129
},
"orientation": {
"x": 0.0,
"y": 1.0,
"z": 0.0,
"w": 0.0
}
},
"distance": 0.015
}
},
"placements": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"w": 0.0
}
}
}
}
}

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

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

29
sdf_models/models/box1/model.sdf Normal file
View file

@ -0,0 +1,29 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="box1">
<pose>0 0 0.015 0 0 0</pose>
<link name="link">
<gravity>0</gravity>
<collision name="collision">
<geometry>
<box>
<size>0.03 0.03 0.03</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.03 0.03 0.03</size>
</box>
</geometry>
<material>
<ambient>0.5 0.75 0 1</ambient>
<diffuse>0.7 0.9 0 1</diffuse>
<specular>0.2 0.2 0.2 64</specular>
<emissive>0.1 0 0.1 1</emissive>
</material>
</visual>
</link>
</model>
</sdf>

40
sdf_models/models/box12/frames.json Normal file
View file

@ -0,0 +1,40 @@
{
"label": "box12",
"type": "assembly",
"include": ["box1", "box2"],
"features": {
"grasp-poses": {
"Gripper_for_Part__Feature059": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.129
},
"orientation": {
"x": 0.0,
"y": 1.0,
"z": 0.0,
"w": 0.0
}
},
"distance": 0.015
}
},
"placements": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"w": 0.0
}
}
}
}
}

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

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

19
sdf_models/models/box12/model.sdf Normal file
View file

@ -0,0 +1,19 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="box12">
<pose>0 0 0.015 0 0 0</pose>
<link name="box1">
<pose>0 0 0 0 0 0</pose>
<gravity>0</gravity>
</link>
<link name="box2">
<pose>0 0 0.03 0 0 0</pose>
<gravity>0</gravity>
</link>
<joint name="joint" type="fixed">
<parent>box1</parent>
<child>box2</child>
<pose>0.0 0.0 0.06 0.0 0.0 0.0</pose>
</joint>
</model>
</sdf>

40
sdf_models/models/box2/frames.json Normal file
View file

@ -0,0 +1,40 @@
{
"label": "box2",
"type": "component",
"include": [],
"features": {
"grasp-poses": {
"Gripper_for_Part__Feature058": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.129
},
"orientation": {
"x": 0.0,
"y": 1.0,
"z": 0.0,
"w": 0.0
}
},
"distance": 0.015
}
},
"placements": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"w": 0.0
}
}
}
}
}

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

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

29
sdf_models/models/box2/model.sdf Normal file
View file

@ -0,0 +1,29 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="box2">
<pose>0 0 0.015 0 0 0</pose>
<link name="link">
<gravity>0</gravity>
<collision name="collision">
<geometry>
<box>
<size>0.03 0.03 0.03</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.03 0.03 0.03</size>
</box>
</geometry>
<material>
<ambient>0.1 0.75 0 5</ambient>
<diffuse>0.9 0.7 0 1</diffuse>
<specular>0.4 0.4 0.4 64</specular>
<emissive>0.2 0 0.2 2</emissive>
</material>
</visual>
</link>
</model>
</sdf>

51
sdf_models/models/output_shaft_211118/frames.json
View file

@ -1,51 +0,0 @@
{
"label": "output_shaft_211118",
"placement": {
"position": {
"x": -0.0,
"y": -0.0,
"z": -0.0
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 1.0,
"w": 0.0
}
},
"features": {
"grasp-poses": {
"Gripper_for_Part__Feature057": {
"placement": {
"position": {
"x": -4e-19,
"y": -0.0815,
"z": 2.2099999999999998e-17
},
"orientation": {
"x": 0.7133549208900113,
"y": 0.4955425091967418,
"z": 0.4955425091967417,
"w": 4.38085841046605
}
},
"distance": 0.025
}
},
"placements": {
"placement": {
"position": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0,
"w": 0.0
}
}
}
}
}

51
sdf_models/models/output_shaft_211118/meshes/output_shaft_211118.dae
View file

File diff suppressed because one or more lines are too long

11
sdf_models/models/output_shaft_211118/model.config
View file

@ -1,11 +0,0 @@
<?xml version="1.0" ?>
<model>
<name>output_shaft_211118</name>
<version>Version</version>
<sdf version="1.5">model.sdf</sdf>
<author>
<name>Author</name>
<email>Email</email>
</author>
<description>Comment</description>
</model>

39
sdf_models/models/output_shaft_211118/model.sdf
View file

@ -1,39 +0,0 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="output_shaft_211118">
<pose>0.000000 0.021391 0.020764 0.000000 0.000000 0.000000</pose>
<static>false</static>
<self_collide>false</self_collide>
<link name="output_shaft_211118">
<pose>0.021818 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
<inertial>
<pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
<mass>0.020176</mass>
<inertia>
<ixx>0.000004</ixx>
<ixy>-0.000000</ixy>
<ixz>-0.000000</ixz>
<iyy>0.000007</iyy>
<iyz>0.000000</iyz>
<izz>0.000007</izz>
</inertia>
</inertial>
<visual name="output_shaft_211118_visual">
<pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
<geometry>
<mesh>
<uri>model://output_shaft_211118/meshes/output_shaft_211118.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="output_shaft_211118_collision">
<pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
<geometry>
<mesh>
<uri>model://output_shaft_211118/meshes/output_shaft_211118.dae</uri>
</mesh>
</geometry>
</collision>
</link>
</model>
</sdf>