Skip to content
Snippets Groups Projects
Commit 5dfa01c1 authored by KingMaZito's avatar KingMaZito
Browse files

behavioural analysis in ferdinand's package

parent 5ed8d517
No related branches found
No related tags found
No related merge requests found
...@@ -11,6 +11,7 @@ ...@@ -11,6 +11,7 @@
#include "impl/abstract_robot.h" #include "impl/abstract_robot.h"
#include "impl/robot.h" #include "impl/robot.h"
#include <gb_grasp/VoxelManager.h> #include <gb_grasp/VoxelManager.h>
#include <gb_grasp/GraspMap.h>
#include "reader/abstract_param_reader.h" #include "reader/abstract_param_reader.h"
#include "reader/robot_reader.h" #include "reader/robot_reader.h"
#include "reader/cuboid_reader.h" #include "reader/cuboid_reader.h"
...@@ -22,6 +23,9 @@ class Moveit_grasp_mediator : public Moveit_mediator{ ...@@ -22,6 +23,9 @@ class Moveit_grasp_mediator : public Moveit_mediator{
std::shared_ptr<planning_pipeline::PlanningPipeline> planning_pipeline_; std::shared_ptr<planning_pipeline::PlanningPipeline> planning_pipeline_;
std::shared_ptr<VoxelManager> voxel_manager_; std::shared_ptr<VoxelManager> voxel_manager_;
std::string grasp_ns_; std::string grasp_ns_;
boost::dynamic_bitset<> voxel_environment_;
std::vector<GraspMap> grasp_maps_;
// Readers // Readers
std::unique_ptr<Abstract_param_reader> robot_reader_; std::unique_ptr<Abstract_param_reader> robot_reader_;
...@@ -29,53 +33,7 @@ class Moveit_grasp_mediator : public Moveit_mediator{ ...@@ -29,53 +33,7 @@ class Moveit_grasp_mediator : public Moveit_mediator{
std::unique_ptr<Abstract_param_reader> cuboid_reader_; std::unique_ptr<Abstract_param_reader> cuboid_reader_;
public: public:
Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh) Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
: Moveit_mediator(objects, pub, nh)
, robot_reader_(std::make_unique<Robot_reader>(nh))
, wing_reader_(std::make_unique<Wing_reader>(nh))
, cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
visual_tools_->loadMarkerPub();
visual_tools_->loadRemoteControl();
visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);
visual_tools_->loadRobotStatePub("/display_robot_state");
visual_tools_->setRobotStateTopic("/robot_state_publisher");
visual_tools_->loadTrajectoryPub("/display_planned_path");
visual_tools_->loadSharedRobotState();
visual_tools_->enableBatchPublishing();
visual_tools_->deleteAllMarkers();
visual_tools_->removeAllCollisionObjects();
visual_tools_->trigger();
nh_->getParam("/grasps_ns", grasp_ns_);
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();
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();
for (int i = 0; i < robots_.size(); i++){
for(object_data& w : wd[i].first){
w.pose_ = robots_[i]->tf().inverse() * w.pose_;
}
robots_[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
}
set_wings(wd);
for (int i = 0; i < robots_.size(); i++){
std::bitset<3> bs = wd[i].second;
build_wings(bs, i);
}
scene_setup();
};
void mediate() override; void mediate() override;
...@@ -83,6 +41,21 @@ class Moveit_grasp_mediator : public Moveit_mediator{ ...@@ -83,6 +41,21 @@ class Moveit_grasp_mediator : public Moveit_mediator{
void manipulate_grasp_data(Moveit_robot* robot); void manipulate_grasp_data(Moveit_robot* robot);
void scene_setup(); void scene_setup();
void manipulate_mapspace();
bool generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth, double& y_width, double& z_height);
bool getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose, robot_state::RobotState& solution, const std::string& link_name);
bool planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
moveit_msgs::MotionPlanResponse& pre_approach_plan, const std::string& object_name);
void visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
const moveit_msgs::MotionPlanResponse& pre_approach_plan);
void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt);
bool planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan);
bool isStateValid(const planning_scene::PlanningScene* planning_scene,
const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state,
const robot_model::JointModelGroup* group, const double* ik_solution);
}; };
......
...@@ -68,29 +68,7 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -68,29 +68,7 @@ class Moveit_mediator : public Abstract_mediator{
public: public:
Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh) Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
: Abstract_mediator(objects, pub), nh_(nh)
, sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
, cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
, psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
, mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
, planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
, planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description")){
robot_model_loader::RobotModelLoaderPtr robot_model_loader;
robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
robot_model_ = robot_model_loader->getModel();
ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
// planner
sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
// cartesian
cartesian_planner_->setMaxVelocityScaling(1.0);
cartesian_planner_->setMaxAccelerationScaling(1.0);
cartesian_planner_->setStepSize(.01);
};
void setup_task(); void setup_task();
inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;}; inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;};
......
...@@ -39,6 +39,8 @@ class Moveit_robot : public Robot{ ...@@ -39,6 +39,8 @@ class Moveit_robot : public Robot{
inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;} inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;}
inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_hand() {return mgi_hand_;} inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_hand() {return mgi_hand_;}
inline std::shared_ptr<MapGenerator> grasp_map_generator() {return grasp_map_generator_;}
inline std::map<std::string, std::string>& map(){return map_;} inline std::map<std::string, std::string>& map(){return map_;}
inline void set_grasp_map_generator(std::shared_ptr<MapGenerator>const& d ){ grasp_map_generator_ = d;} inline void set_grasp_map_generator(std::shared_ptr<MapGenerator>const& d ){ grasp_map_generator_ = d;}
......
...@@ -12,16 +12,16 @@ ...@@ -12,16 +12,16 @@
class Cuboid_reader : public Abstract_param_reader{ class Cuboid_reader : public Abstract_param_reader{
protected: protected:
std::vector<Cuboid> cuboid_bin_; std::vector<Cuboid> cuboid_box_;
std::vector<Cuboid> cuboid_obstacle_; std::vector<Cuboid> cuboid_obstacle_;
public: public:
Cuboid_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){read();} Cuboid_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){read();}
inline void set_cuboid_bin_data(std::vector<Cuboid>& cuboid_data) {cuboid_bin_ = cuboid_data;} inline void set_cuboid_bin_data(std::vector<Cuboid>& cuboid_data) {cuboid_box_ = cuboid_data;}
inline void set_cuboid_obstacle_data(std::vector<Cuboid>& cuboid_data) {cuboid_obstacle_ = cuboid_data;} inline void set_cuboid_obstacle_data(std::vector<Cuboid>& cuboid_data) {cuboid_obstacle_ = cuboid_data;}
inline std::vector<Cuboid> cuboid_bin() {return cuboid_bin_;} inline std::vector<Cuboid> cuboid_bin() {return cuboid_box_;}
inline std::vector<Cuboid> cuboid_obstacle() {return cuboid_obstacle_;} inline std::vector<Cuboid> cuboid_obstacle() {return cuboid_obstacle_;}
void read() override; void read() override;
......
...@@ -2,57 +2,64 @@ ...@@ -2,57 +2,64 @@
<!-- Launch rviz.launch before running this demo to see visalizations --> <!-- Launch rviz.launch before running this demo to see visalizations -->
<arg name="result" default="dummy/-562289182.yaml"/> <arg name="result" default="dummy/-562289182.yaml"/>
<arg name="grasps_ns" default="grasps" /> <arg name="grasps_ns" default="grasps" />
<arg name="gripper" default="two_finger" doc="must be one of 'suction' or 'two_finger'" />
<arg name="planner" default="ompl" />
<arg name="moveit_controller_manager" default="fake" /> <arg name="moveit_controller_manager" default="fake" />
<arg name="fake_execution_type" default="interpolate" /> <arg name="fake_execution_type" default="interpolate" />
<arg name="use_gui" default="false" /> <arg name="use_gui" default="false" />
<arg name="use_rviz" default="true" /> <arg name="use_rviz" default="true" />
<!-- GDB Debug Arguments -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix"
value="gdb -x $(find moveit_grasps)/launch/gdb_settings.gdb --ex run --args" />
<!-- Verbose mode -->
<arg name="verbose" default="false" />
<arg unless="$(arg verbose)" name="command_args" value="" />
<arg if="$(arg verbose)" name="command_args" value="--verbose" />
<!-- PANDA --> <!-- Callgrind Arguments -->
<include file="$(find ceti_double)/launch/planning_context.launch"> <arg name="callgrind" default="false" />
<arg name="load_robot_description" value="true"/> <arg unless="$(arg callgrind)" name="launch_prefix2" value="" />
<arg name="scene" value="$(arg result)" /> <arg if="$(arg callgrind)" name="launch_prefix2" value="valgrind --tool=callgrind --collect-atstart=no" />
</include>
<!-- this is needed because moveit grasps uses an additional joint --> <!-- Valgrind Arguments -->
<arg name="robot_description" default="robot_description"/> <arg name="valgrind" default="false" />
<param name="$(arg robot_description)" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/double_panda_arm_two_finger_hand.urdf.xacro' 'scene:=$(arg result)'"/> <arg unless="$(arg valgrind)" name="launch_prefix3" value="" />
<arg if="$(arg valgrind)" name="launch_prefix3" value="valgrind --tool=memcheck --leak-check=full --show-leak-kinds=all --track-origins=yes -v" />
<!-- Publish world frame -->
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world base_1" />
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_2" args="0 0 0 0 0 0 world base_2" />
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base_1" />
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world base_2" />
<!-- PANDA -->
<include file="$(find moveit_grasps)/launch/panda_planning_context.launch">
<arg name="load_robot_description" value="true"/>
<arg name="gripper" value="$(arg gripper)"/>
<arg name="scene" value="$(arg result)"/>
</include>
<!-- Load Rviz --> <!-- PANDA -->
<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" <include file="$(find moveit_grasps)/launch/load_panda.launch">
args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen"> <arg name="gripper" value="$(arg gripper)"/>
<rosparam command="load" file="$(find ceti_double)/config/kinematics.yaml"/> <arg name="scene" value="$(arg result)" />
</node> </include>
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find ceti_double)/launch/move_group.launch"> <include file="$(find ceti_double)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/> <arg name="allow_trajectory_execution" value="true"/>
<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" /> <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
<arg name="fake_execution_type" value="$(arg fake_execution_type)"/> <arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
<arg name="info" value="true"/> <arg name="info" value="true"/>
<arg name="debug" value="true"/> <arg name="debug" value="$(arg debug)"/>
<arg name="pipeline" value="ompl" /> <arg name="pipeline" value="$(arg planner)"/>
<arg name="load_robot_description" value="false"/>
<arg name="scene" value="$(arg result)" /> <arg name="scene" value="$(arg result)" />
</include> </include>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
</node>
<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" /> <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
...@@ -66,8 +73,14 @@ ...@@ -66,8 +73,14 @@
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/> <rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
</node> </node>
<!-- Load Rviz -->
<node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen">
</node>
<!-- Planning Functionality --> <!-- Planning Functionality -->
<arg name="planner" default="ompl" />
<include ns="grasp_cell_routine" file="$(find ceti_double)/launch/planning_pipeline.launch.xml"> <include ns="grasp_cell_routine" file="$(find ceti_double)/launch/planning_pipeline.launch.xml">
<arg name="pipeline" value="$(arg planner)" /> <arg name="pipeline" value="$(arg planner)" />
</include> </include>
......
...@@ -45,6 +45,336 @@ Toolbars: ...@@ -45,6 +45,336 @@ Toolbars:
Visualization Manager: Visualization Manager:
Class: "" Class: ""
Displays: Displays:
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /rviz_visual_tools
Name: MarkerArray
Namespaces:
Axis: true
Cuboid: true
Queue Size: 100
Value: true
- Class: moveit_rviz_plugin/Trajectory
Color Enabled: false
Enabled: true
Interrupt Display: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_1_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_tool_center_point:
Alpha: 1
Show Axes: false
Show Trail: false
panda_2_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_hand_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_2_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_tool_center_point:
Alpha: 1
Show Axes: false
Show Trail: false
world:
Alpha: 1
Show Axes: false
Show Trail: false
Loop Animation: false
Name: Trajectory
Robot Alpha: 0.5
Robot Color: 150; 50; 150
Robot Description: robot_description
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 0.05 s
Trail Step Size: 1
Trajectory Topic: /display_planned_path
Use Sim Time: false
Value: true
- Attached Body Color: 150; 50; 150
Class: moveit_rviz_plugin/RobotState
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_1_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_1_tool_center_point:
Alpha: 1
Show Axes: false
Show Trail: false
panda_2_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_hand_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_2_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_2_tool_center_point:
Alpha: 1
Show Axes: false
Show Trail: false
world:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotState
Robot Alpha: 1
Robot Description: robot_description
Robot State Topic: /display_robot_state
Show All Links: true
Show Highlights: true
Value: true
Visual Enabled: true
- Alpha: 0.5 - Alpha: 0.5
Cell Size: 1 Cell Size: 1
Class: rviz/Grid Class: rviz/Grid
......
...@@ -21,5 +21,12 @@ ...@@ -21,5 +21,12 @@
{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, { 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.104997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.104997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': .2, 'y': .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
{ 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B
{ 'id': 'green1', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'green2', 'type': 'BOX', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'red2', 'type': 'BOX', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } }
]} ]}
\ No newline at end of file
...@@ -21,5 +21,12 @@ ...@@ -21,5 +21,12 @@
{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, { 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
{ 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
{ 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': .2, 'y': .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
{ 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B
{ 'id': 'green1', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'green2', 'type': 'BOX', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'red2', 'type': 'BOX', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } }
]} ]}
\ No newline at end of file
...@@ -43,16 +43,13 @@ int main(int argc, char **argv){ ...@@ -43,16 +43,13 @@ int main(int argc, char **argv){
Moveit_mediator* moveit_mediator = dynamic_cast<Moveit_mediator*>(mediator); Moveit_mediator* moveit_mediator = dynamic_cast<Moveit_mediator*>(mediator);
std::unique_ptr<Abstract_param_reader> robot_reader(new Robot_reader(nh)); std::unique_ptr<Abstract_param_reader> robot_reader(new Robot_reader(nh));
robot_reader->read();
auto rd = static_cast<Robot_reader*>(robot_reader.get())->robot_data(); auto rd = static_cast<Robot_reader*>(robot_reader.get())->robot_data();
for (int i = 0; i < rd.size() ;i++) mediator->connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_)); for (int i = 0; i < rd.size() ;i++) mediator->connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
std::unique_ptr<Abstract_param_reader> wing_reader(new Wing_reader(nh)); std::unique_ptr<Abstract_param_reader> wing_reader(new Wing_reader(nh));
wing_reader->read();
auto wd = static_cast<Wing_reader*>(wing_reader.get())->wing_data(); auto wd = static_cast<Wing_reader*>(wing_reader.get())->wing_data();
std::unique_ptr<Abstract_param_reader> cuboid_reader(new Cuboid_reader(nh)); std::unique_ptr<Abstract_param_reader> cuboid_reader(new Cuboid_reader(nh));
cuboid_reader->read();
auto cbd = static_cast<Cuboid_reader*>(cuboid_reader.get())->cuboid_bin(); auto cbd = static_cast<Cuboid_reader*>(cuboid_reader.get())->cuboid_bin();
auto cod = static_cast<Cuboid_reader*>(cuboid_reader.get())->cuboid_obstacle(); auto cod = static_cast<Cuboid_reader*>(cuboid_reader.get())->cuboid_obstacle();
...@@ -79,6 +76,7 @@ int main(int argc, char **argv){ ...@@ -79,6 +76,7 @@ int main(int argc, char **argv){
//moveit::planning_interface::PlanningSceneInterface::applyCollisionObject() //moveit::planning_interface::PlanningSceneInterface::applyCollisionObject()
ceti1->notify(); ceti1->notify();
ceti2->notify(); ceti2->notify();
moveit_mediator->mediate(); moveit_mediator->mediate();
......
This diff is collapsed.
...@@ -883,3 +883,36 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll ...@@ -883,3 +883,36 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll
} }
Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
: Abstract_mediator(objects, pub), nh_(nh)
, sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
, cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
, psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
, mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
, planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
, planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description")){
if (planning_scene_monitor_->getPlanningScene()){
planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
"planning_scene");
planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
} else {
ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
return;
}
robot_model_loader::RobotModelLoaderPtr robot_model_loader;
robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
robot_model_ = robot_model_loader->getModel();
ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
// planner
sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
// cartesian
cartesian_planner_->setMaxVelocityScaling(1.0);
cartesian_planner_->setMaxAccelerationScaling(1.0);
cartesian_planner_->setStepSize(.01);
};
...@@ -38,7 +38,7 @@ void Cuboid_reader::read(){ ...@@ -38,7 +38,7 @@ void Cuboid_reader::read(){
o.z_heigth = size.getZ(); o.z_heigth = size.getZ();
cuboid_bin_.push_back(o); cuboid_box_.push_back(o);
ROS_INFO("--- Object: %s --- Type: BOX ---", o.Name.c_str()); ROS_INFO("--- Object: %s --- Type: BOX ---", o.Name.c_str());
ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ()); ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW()); ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment