From 52f9d78ae445d1dda29b29e55d6390e95ebe2d7f Mon Sep 17 00:00:00 2001
From: SebastianEbert <sebastian.ebert@tu-dresden.de>
Date: Wed, 27 May 2020 11:55:47 +0200
Subject: [PATCH] fixed environment generation, generalized data-formats
---
launch/simulation.launch | 4 ++--
src/grasping/environment_util.cpp | 10 +++++-----
src/grasping/environment_util.h | 2 +-
src/grasping/grasp_service.cpp | 16 +++++++++++-----
srv/PickObject.srv | 4 +---
5 files changed, 20 insertions(+), 16 deletions(-)
diff --git a/launch/simulation.launch b/launch/simulation.launch
index b25573f..58ed7ef 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -28,7 +28,7 @@
<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
<!-- Load teaching robot information from YAML file to parameter server -->
- <rosparam file="$(find panda_teaching)/config/robot_information.yaml" command="load" />
+ <!--<rosparam file="$(find panda_teaching)/config/robot_information.yaml" command="load" />-->
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
@@ -61,5 +61,5 @@
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
<node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" />
- <!--<node pkg="panda_teaching" type="grasp_service" name="grasp_service" output="screen" />-->
+ <!--<node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" />-->
</launch>
diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp
index f48c394..2d22d60 100644
--- a/src/grasping/environment_util.cpp
+++ b/src/grasping/environment_util.cpp
@@ -5,7 +5,7 @@
#include "environment_util.h"
-void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects) {
+void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {
moveit_msgs::CollisionObject plate;
@@ -13,12 +13,12 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &
plate.id = "plate";
plate.primitives.resize(1);
- plate.primitives[0].type = collision_objects[1].primitives[0].BOX;
+ plate.primitives[0].type = 1u;
plate.primitives[0].dimensions.resize(3);
- plate.primitives[0].dimensions[0] = 3.0;
- plate.primitives[0].dimensions[1] = 3.0;
- plate.primitives[0].dimensions[2] = 0.1;
+ plate.primitives[0].dimensions[0] = x_dimension;
+ plate.primitives[0].dimensions[1] = y_dimension;
+ plate.primitives[0].dimensions[2] = 0.1;
plate.primitive_poses.resize(1);
plate.primitive_poses[0].position.x = 0;
plate.primitive_poses[0].position.y = 0;
diff --git a/src/grasping/environment_util.h b/src/grasping/environment_util.h
index 9fd8102..b24d9df 100644
--- a/src/grasping/environment_util.h
+++ b/src/grasping/environment_util.h
@@ -19,7 +19,7 @@ class EnvironmentUtil {
public:
- void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects);
+ void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension);
void constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
double object_x_dimension, double object_y_dimension, double object_z_dimension,
diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp
index 2c73051..ad4b2c8 100644
--- a/src/grasping/grasp_service.cpp
+++ b/src/grasping/grasp_service.cpp
@@ -26,10 +26,16 @@ bool pickObject(panda_grasping::PickObject::Request &req,
EnvironmentUtil env_util;
std::vector<moveit_msgs::CollisionObject> collision_objects;
- env_util.constructPlate(collision_objects);
- env_util.constructSupport(collision_objects, "pick_support", req.x_dimension, req.y_dimension, req.z_dimension, req.pose);
+ ROS_INFO("Adding plate.");
+ env_util.constructPlate(collision_objects, 3.0, 3.0);
+
+ ROS_INFO("Adding support.");
+ env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, req.dimensions.z, req.pose);
+
+ ROS_INFO("Applying to scene.");
planning_scene_interface.applyCollisionObjects(collision_objects);
+ res.success = true;
return true;
}
@@ -45,8 +51,8 @@ int main(int argc, char **argv) {
ros::init(argc, argv, "grasp_service");
ros::NodeHandle n("panda_grasping");
- ros::AsyncSpinner spinner(1);
- spinner.start();
+ // ros::AsyncSpinner spinner(2);
+ // spinner.start();
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroupInterface group("panda_arm");
@@ -54,7 +60,7 @@ int main(int argc, char **argv) {
ros::ServiceServer pick_object_service = n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>("PickObjectService",
boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
- ros::ServiceServer place_object_service = n.advertiseService("PlaceObjectService", placeObject);
+ //ros::ServiceServer place_object_service = n.advertiseService("PlaceObjectService", placeObject);
ROS_INFO("Ready to grasp.");
ros::spin();
diff --git a/srv/PickObject.srv b/srv/PickObject.srv
index 5f9340b..d0e99d0 100644
--- a/srv/PickObject.srv
+++ b/srv/PickObject.srv
@@ -1,6 +1,4 @@
geometry_msgs/Pose pose
-float64 x_dimension
-float64 y_dimension
-float64 z_dimension
+geometry_msgs/Vector3 dimensions
---
bool success
\ No newline at end of file
--
GitLab