diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp index 3a34852a92d3b24c9251e2960061fd2117b8fa94..360de2f9a6988cd1157a2368df73df066c438a8c 100644 --- a/src/tron_adapter.cpp +++ b/src/tron_adapter.cpp @@ -9,6 +9,8 @@ #include <cooperation_msgs/PressureMsg.h> #include <moveit_msgs/PickupActionResult.h> #include <moveit_msgs/MoveItErrorCodes.h> +#include <moveit_msgs/MoveGroupActionResult.h> +#include <control_msgs/FollowJointTrajectoryAction.h> const std::string IP = "127.0.0.1"; const uint16_t PORT = 8080; @@ -357,10 +359,11 @@ void report_now(Channel& chan, uint16_t var_count = 0, int32_t *vars = nullptr){ void report_now(std::string chan, uint16_t var_count = 0, int32_t *vars = nullptr){ for (Mapping& map : mappings) - if (map.channel.name == chan) { + if (map.channel.name == chan && !map.channel.is_input) { report_now(map.channel, var_count, vars); return; } + throw "could not report to channel"; } // callback reports to TRON like defined in mappings @@ -404,6 +407,12 @@ void on_pickup_result(const moveit_msgs::PickupActionResult::ConstPtr &msg) { else report_now("pickup_fail"); } +void on_move_result(const control_msgs::FollowJointTrajectoryActionResult::ConstPtr &msg){ + if (msg->result.error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL) + report_now("move_success"); + else report_now("move_fail"); +} + void configuration_phase(ros::NodeHandle& nh){ /* note: for configuration phase maximum message length is 256 Bytes, therefore heap allocation can be avoided most of the time in called functions */ @@ -418,8 +427,20 @@ void configuration_phase(ros::NodeHandle& nh){ output_subscribers.push_back(nh.subscribe("/pressure", 10, on_pressure)); map = Mapping("/pickup/result", "pickup_fail", false); + mappings.push_back(map); map = Mapping("/pickup/result", "pickup_success", false); + mappings.push_back(map); output_subscribers.push_back(nh.subscribe("/pickup/result", 10, on_pickup_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); + map = Mapping("/position_joint_trajectory_controller/follow_joint_trajectory/result", "move_success", false); + mappings.push_back(map); + output_subscribers.push_back(nh.subscribe("/position_joint_trajectory_controller/follow_joint_trajectory/result", 10, on_move_result)); + + // TODO: place and variables + // not obvious in documentation: local variables are not supported // add_var_to_channel(socketfd, "ausloesen", "lokal"); uint64_t microseconds = 1000000; // one second