No commit message

No commit message

src/tron_adapter.cpp

@@ -7,7 +7,8 @@
 #include <boost/make_shared.hpp>
 #include <memory>
 #include <cooperation_msgs/PressureMsg.h>
-#include <moveit_msgs/PickupActionResult.h>
+#include <moveit_msgs/PickupAction.h>
+#include <moveit_msgs/PlaceAction.h>
 #include <moveit_msgs/MoveItErrorCodes.h>
 #include <moveit_msgs/MoveGroupActionResult.h>
 #include <control_msgs/FollowJointTrajectoryAction.h>
@@ -407,6 +408,12 @@ void on_pickup_result(const moveit_msgs::PickupActionResult::ConstPtr &msg) {
     else report_now("pickup_fail");
 }
 
+void on_place_result(const moveit_msgs::PlaceActionResult::ConstPtr &msg) {
+    if (msg->result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
+        report_now("place_success");
+    else report_now("place_fail");
+}
+
 void on_move_result(const control_msgs::FollowJointTrajectoryActionResult::ConstPtr &msg){
     if (msg->result.error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
         report_now("move_success");
@@ -432,6 +439,12 @@ void configuration_phase(ros::NodeHandle& nh){
     mappings.push_back(map);
     output_subscribers.push_back(nh.subscribe("/pickup/result", 10, on_pickup_result));
 
+    map = Mapping("/place/result", "place_fail", false);
+    mappings.push_back(map);
+    map = Mapping("/place/result", "place_success", false);
+    mappings.push_back(map);
+    output_subscribers.push_back(nh.subscribe("/place/result", 10, on_place_result));
+
     // there is a separate controller for hand movement, this one is for arm movement
     map = Mapping("/position_joint_trajectory_controller/follow_joint_trajectory/result", "move_fail", false);
     mappings.push_back(map);
@@ -445,7 +458,7 @@ void configuration_phase(ros::NodeHandle& nh){
     // add_var_to_channel(socketfd, "ausloesen", "lokal");
     uint64_t microseconds = 1000000; // one second
     // documentation states 2 signed integers are used for some reason
-    set_time_unit_and_timeout(microseconds, 300);
+    set_time_unit_and_timeout(microseconds, 600);
 
     // wait till subscribers initialized
     for (ros::Publisher& pub : input_publishers) {