Skip to content
Snippets Groups Projects
Commit 97007c06 authored by KingMaZito's avatar KingMaZito
Browse files

update acm, not perfect, but ok

parent 7aa3f9fa
Branches
No related tags found
No related merge requests found
Expand all Show whitespace changes
Inline Side-by-side
Showing
with 410 additions and 272 deletions
.vscode/c_cpp_properties.json 0 → 100644
+ 39
− 0
View file @ 97007c06
{
"configurations": [
{
"browse": {
"databaseFilename": "${default}",
"limitSymbolsToIncludedHeaders": false
},
"includePath": [
"/home/matteo/ccf_sim_workspace/devel/include/**",
"/opt/ros/noetic/include/**",
"/home/matteo/ccf_sim_workspace/src/BehaviorTree.CPP/include/**",
"/home/matteo/ccf_sim_workspace/src/ccf/include/**",
"/home/matteo/ccf_sim_workspace/src/ccf_immersive_sorting/include/**",
"/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_grasp_plugin/include/**",
"/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_state_plugins/include/**",
"/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_test_tools/include/**",
"/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_version_helpers/include/**",
"/home/matteo/ccf_sim_workspace/src/gazebo_pkgs/gazebo_world_plugin_loader/include/**",
"/home/matteo/ccf_sim_workspace/src/gb-grasp-auerswald/include/**",
"/home/matteo/ccf_sim_workspace/src/moveit-grasps-reduced/include/**",
"/home/matteo/ccf_sim_workspace/src/moveit_task_constructor/core/include/**",
"/home/matteo/ccf_sim_workspace/src/moveit_task_constructor/demo/include/**",
"/home/matteo/ccf_sim_workspace/src/multi_cell_builder/include/**",
"/home/matteo/ccf_sim_workspace/src/general_message_packages/object_msgs_tools/include/**",
"/home/matteo/ccf_sim_workspace/src/panda_grasping/include/**",
"/home/matteo/ccf_sim_workspace/src/panda_util/include/**",
"/home/matteo/ccf_sim_workspace/src/moveit_task_constructor/rviz_marker_tools/include/**",
"/home/matteo/ccf_sim_workspace/src/simulation_util/include/**",
"/usr/include/**"
],
"name": "ROS",
"intelliSenseMode": "gcc-x64",
"compilerPath": "/usr/bin/gcc",
"cStandard": "gnu11",
"cppStandard": "c++14"
}
],
"version": 4
}
\ No newline at end of file
.vscode/settings.json 0 → 100644
+ 6
− 0
View file @ 97007c06
{
"python.autoComplete.extraPaths": [
"/home/matteo/ccf_sim_workspace/devel/lib/python3/dist-packages",
"/opt/ros/noetic/lib/python3/dist-packages"
]
}
\ No newline at end of file
CMakeLists.txt
+ 6
− 0
View file @ 97007c06
......@@ -103,6 +103,8 @@ src/reader/abstract_param_reader.cpp
src/reader/robot_reader.cpp
src/reader/wing_reader.cpp
src/reader/cuboid_reader.cpp
src/reader/job_reader.cpp
)
add_executable(grasp_cell_routine src/grasp_cell_routine.cpp
......@@ -122,6 +124,8 @@ src/reader/abstract_param_reader.cpp
src/reader/robot_reader.cpp
src/reader/wing_reader.cpp
src/reader/cuboid_reader.cpp
src/reader/job_reader.cpp
)
......@@ -129,6 +133,8 @@ add_executable(mtc2taskspace src/mtc2taskspace.cpp
src/reader/abstract_param_reader.cpp
src/reader/task_space_reader.cpp
src/reader/robot_reader.cpp
src/reader/job_reader.cpp
)
......
include/impl/moveit_grasp_mediator.h
+ 5
− 1
View file @ 97007c06
......@@ -15,6 +15,8 @@
#include "reader/abstract_param_reader.h"
#include "reader/robot_reader.h"
#include "reader/cuboid_reader.h"
#include "reader/job_reader.h"
......@@ -28,9 +30,11 @@ class Moveit_grasp_mediator : public Moveit_mediator{
// Readers
std::unique_ptr<Abstract_param_reader> robot_reader_;
std::unique_ptr<Robot_reader> robot_reader_;
std::unique_ptr<Abstract_param_reader> wing_reader_;
std::unique_ptr<Abstract_param_reader> cuboid_reader_;
std::unique_ptr<Job_reader> job_reader_;
public:
Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
......
include/reader/abstract_param_reader.h
+ 0
− 1
View file @ 97007c06
......@@ -17,7 +17,6 @@ struct object_data {
};
struct job_data {
std::string name_;
std::vector<tf2::Transform> jobs_;
};
......
include/reader/job_reader.h
+ 4
− 6
View file @ 97007c06
......@@ -10,17 +10,15 @@
class Job_reader : public Abstract_param_reader{
protected:
std::vector<job_data> job_data_;
std::string name_;
std::map<std::string, std::vector<job_data>> job_data_;
public:
Job_reader(std::shared_ptr<ros::NodeHandle> const& d, std::string const& name)
Job_reader(std::shared_ptr<ros::NodeHandle> const& d)
: Abstract_param_reader(d)
, name_(name)
{read();}
inline void set_job_data(std::vector<job_data>& robot_data) {job_data_ = robot_data;}
inline std::vector<job_data> robot_data() {return job_data_;}
inline void set_job_data(std::map<std::string, std::vector<job_data>>& robot_data) {job_data_ = robot_data;}
inline std::map<std::string, std::vector<job_data>>& robot_data() {return job_data_;}
void read() override;
};
......
jobs/dummy.yaml 0 → 100644
+ 56
− 0
View file @ 97007c06
{ 'task':
{'groups' : [
{ 'name': 'panda_arm1', 'jobs':[[
{ 'pos': { 'x': -0.300000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.300000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.200000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.200000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.700000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.600000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': -0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': -0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': -0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 0.100000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 0.200000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 0.300000,'z': 0.890000 }, 'orientation': { 'w': 1 } }
]]},
{ 'name': 'panda_arm2', 'jobs':[[
{ 'pos': { 'x': 0.100000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.110000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.210000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.200000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.410000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.510000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.300000 ,'y': 1.610000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.300000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.300000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.200000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': -0.200000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 1.910000,'z': 0.890000 }, 'orientation': { 'w': 1 } },
{ 'pos': { 'x': 0.100000 ,'y': 2.010000,'z': 0.890000 }, 'orientation': { 'w': 1 } }
]]}
]
}
}
launch/cell_routine.launch
+ 4
− 0
View file @ 97007c06
<launch>
<arg name="result" default="dummy/-562289182.yaml" />
<arg name="jobs" default="jobs/dummy.yaml" />
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
<!-- this is to change-->
......@@ -7,6 +9,8 @@
<rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
<rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
<rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
<!-- <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/> -->
<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
......
launch/mtc2taskspace.launch 0 → 100644
+ 8
− 0
View file @ 97007c06
<launch>
<arg name="resource" default="jobs/dummy.yaml" />
<rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
<rosparam command="load" file="$(find multi_cell_builder)/$(arg resource)"/>
<node pkg="multi_cell_builder" type="mtc2taskspace" name="mtc2taskspace" output="screen" required="true"/>
</launch>
src/impl/moveit_grasp_mediator.cpp
+ 2
− 2
View file @ 97007c06
......@@ -527,7 +527,7 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf
, robot_reader_(std::make_unique<Robot_reader>(nh))
, wing_reader_(std::make_unique<Wing_reader>(nh))
, cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
// , job_reader_(std::make_unique<Job_reader>(nh)) {
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
visual_tools_->loadMarkerPub();
......@@ -551,7 +551,7 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf
planning_pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, *nh_, "planning_plugin", "request_adapter");
voxel_manager_ = std::make_shared<VoxelManager>(*nh_, grasp_ns_.c_str() ,visual_tools_,planning_scene_monitor_);
auto rd = static_cast<Robot_reader*>(robot_reader_.get())->robot_data();
auto rd = robot_reader_->robot_data();
for (int i = 0; i < rd.size() ;i++) connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
auto wd = static_cast<Wing_reader*>(wing_reader_.get())->wing_data();
......
src/impl/moveit_mediator.cpp
+ 237
− 260
View file @ 97007c06
This diff is collapsed.
src/mtc2taskspace.cpp
+ 5
− 2
View file @ 97007c06
#include "reader/abstract_param_reader.h"
#include "reader/task_space_reader.h"
#include "reader/robot_reader.h"
#include "reader/job_reader.h"
#include <regex>
int main(int argc, char **argv){
ros::init(argc, argv, "mtc2taskspace");
std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
std::unique_ptr<Abstract_param_reader> ts_reader(new Task_space_reader(n));
ts_reader->read();
//std::unique_ptr<Abstract_param_reader> ts_reader(new Task_space_reader(n));
std::unique_ptr<Abstract_param_reader> ts_reader(new Job_reader(n));
return 0;
}
\ No newline at end of file
src/reader/job_reader.cpp 0 → 100644
+ 38
− 0
View file @ 97007c06
#include "reader/job_reader.h"
void Job_reader::read(){
ROS_INFO("--- JOB_READER ---");
XmlRpc::XmlRpcValue resources;
nh_->getParam("/task/groups", resources);
for (int i = 0; i < resources.size(); i++){
std::string robot = resources[i]["name"];
//nh_->getParam("/tasks/groups/" + robot, jobs);
std::vector<job_data> v_jd;
for (int j = 0; j < resources[i]["jobs"].size(); j++){
ROS_INFO("--- %s --- JOB_%i ---", robot.c_str(), j);
job_data jd;
for(int k = 0; k < resources[i]["jobs"][j].size(); k++){
XmlRpc::XmlRpcValue job_data = resources[i]["jobs"][j][k];
tf2::Vector3 pos;
tf2::Quaternion rot;
(job_data["pos"].hasMember("x")) ? pos.setX(float_of(job_data["pos"]["x"])) :pos.setX(0);
(job_data["pos"].hasMember("y")) ? pos.setY(float_of(job_data["pos"]["y"])) :pos.setY(0);
(job_data["pos"].hasMember("z")) ? pos.setZ(float_of(job_data["pos"]["z"])) :pos.setZ(0);
(job_data["orientation"].hasMember("x")) ? rot.setX(float_of(job_data["orientation"]["x"])) :rot.setX(0);
(job_data["orientation"].hasMember("y")) ? rot.setY(float_of(job_data["orientation"]["y"])) :rot.setY(0);
(job_data["orientation"].hasMember("z")) ? rot.setZ(float_of(job_data["orientation"]["z"])) :rot.setZ(0);
(job_data["orientation"].hasMember("w")) ? rot.setW(float_of(job_data["orientation"]["w"])) :rot.setW(0);
ROS_INFO("=> Grab: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
ROS_INFO("=> Grab: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
jd.jobs_.push_back(tf2::Transform(rot, pos));
}
v_jd.push_back(jd);
}
job_data_.insert(std::pair<std::string, std::vector<job_data>>(robot, v_jd));
}
}
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment