No commit message

No commit message

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