No commit message

No commit message

CMakeLists.txt

@@ -4,6 +4,8 @@ project(cobot_1)
 #add_compile_options(-std=c++14)
 set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
+# Wichtig für VSCode debugging
+set(CMAKE_BUILD_TYPE Debug)
 
 # Find catkin macros and libraries if COMPONENTS list like `find_package(catkin REQUIRED COMPONENTS xyz)` is used,
 # also find other catkin packages

launch/cobot1_mock_test.launch

@@ -9,6 +9,7 @@
     <include file="$(find pressure_sensor)/launch/pressure_sensor_mock.launch" />
 
     <node name="Cobot1_TRONAdapter" pkg="cobot_1" type="TRON_Adapter" respawn="false" output="screen"/>
+    
     <node name="Cobot1_Controller" pkg="cobot_1" type="CobotController" respawn="false" output="screen">
         <rosparam command="load" file="$(find cobot_1)/config/cobot1_config.yaml" />
     </node>

src/tron_adapter.cpp

@@ -7,6 +7,8 @@
 #include <boost/make_shared.hpp>
 #include <memory>
 #include <cooperation_msgs/PressureMsg.h>
+#include <moveit_msgs/PickupActionResult.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
 
 const std::string IP = "127.0.0.1";
 const uint16_t PORT = 8080;
@@ -185,7 +187,7 @@ struct Mapping {
         this->channel = *send_channel_decl_msg(channelIsInput, channel).get();
     }
 
-    void add_var_to_mapping(std::string name_tron, int byte_offset, 
+    void add_var_to_mapping(std::string name_tron, int byte_offset = 0, 
                          int32_t (*conv_to_TRON)(byte*, int*) = nullptr,
                          void (*conv_to_topic)(int32_t, byte*, int*) = nullptr){
         add_var_to_channel(this->channel, this->channel.is_input, name_tron);
@@ -353,6 +355,14 @@ void report_now(Channel& chan, uint16_t var_count = 0, int32_t *vars = nullptr){
     acks_missing++;
 }
 
+void report_now(std::string chan, uint16_t var_count = 0, int32_t *vars = nullptr){
+    for (Mapping& map : mappings)
+        if (map.channel.name == chan) {
+            report_now(map.channel, var_count, vars);
+            return;
+        }
+}
+
 // callback reports to TRON like defined in mappings
 template<class T>
 void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){
@@ -381,25 +391,35 @@ void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){
 }
 
 
+int32_t retry_count;
+
 void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) { 
     if (msg->pressed)
-        for (Mapping& map : mappings) {
-            if (map.channel.name == "pressed" && map.channel.is_input == false)
-                report_now(map.channel);
+        report_now("pressed", 1, &retry_count);
 }
+
+void on_pickup_result(const moveit_msgs::PickupActionResult::ConstPtr &msg) {
+    if (msg->result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
+        report_now("pickup_success");
+    else report_now("pickup_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 */
     
+    nh.getParam("/planning/retries", retry_count);
     // note: since we are not an actual client (or server) 
     // we need to use the high level packet::*Action* messages.
     // custom callbacks are implemented in order to not worry about header size
     Mapping map = Mapping("/pressure", "pressed", false);
+    map.add_var_to_mapping("init_retry_count");
     mappings.push_back(map);
     output_subscribers.push_back(nh.subscribe("/pressure", 10, on_pressure));
     
+    map = Mapping("/pickup/result", "pickup_fail", false);
+    map = Mapping("/pickup/result", "pickup_success", false);
+    output_subscribers.push_back(nh.subscribe("/pickup/result", 10, on_pickup_result));
     // not obvious in documentation: local variables are not supported
     // add_var_to_channel(socketfd, "ausloesen", "lokal");
     uint64_t microseconds = 1000000; // one second