fixed environment generation, generalized data-formats

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>

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;

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,

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();

srv/PickObject.srv

 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