diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp index 360de2f9a6988cd1157a2368df73df066c438a8c..dc5ef9c59f23f2ccef0e406ce92184ad1ec34cf4 100644 --- a/src/tron_adapter.cpp +++ b/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) {