No commit message

No commit message

CMakeLists.txt

@@ -65,12 +65,12 @@ include_directories(
         ${CMAKE_CURRENT_BINARY_DIR}
         ${CMAKE_CURRENT_SOURCE_DIR}
 )
-
+add_library(tron_adapter src/tron_adapter.cpp)
 
 add_executable(CobotController src/CobotController.cpp src/Cobot.cpp )
-add_executable(TRON_Adapter src/tron_adapter.cpp)
+add_executable(TRON_Adapter src/cobot_adapter.cpp)
 add_dependencies(CobotController ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Add cmake target dependencies of the executable
 target_link_libraries(CobotController ${catkin_LIBRARIES})
-target_link_libraries(TRON_Adapter ${catkin_LIBRARIES})
+target_link_libraries(TRON_Adapter ${catkin_LIBRARIES} tron_adapter)
\ No newline at end of file

include/tron_adapter.h

@@ -34,20 +34,6 @@ inline bool read_4_bytes_nonblock(int fd, byte *buf);
 int create_connected_socket(std::string IP, uint16_t port);
 
 // ----------------------------------------------------------------------------------------
-// TRON message codes
-extern const byte GET_ERROR_MSG;
-extern const byte DECL_CHAN_INPUT;
-extern const byte DECL_CHAN_OUTPUT;
-extern const byte ADD_VAR_TO_INPUT;
-extern const byte ADD_VAR_TO_OUTPUT;
-extern const byte SET_TIME_UNIT;
-extern const byte SET_TIMEOUT;
-extern const byte REQUEST_START;
-extern const byte ANSWER_START;
-extern const int32_t ACK_SINGLE;
-
-// Socket kept global
-extern int socket_fd;
 
 // represents a channel in Uppaal
 struct Channel {
@@ -62,9 +48,17 @@ struct Channel {
 struct Mapping {
     std::string topic; // ROS topic name
 
+    Channel channel = Channel("", 0, true); // TRON channel
+    // Callback used if TRON sends some message
+    // output callbacks must be specified when subscribing to topics
+    void(*input_callback)(Mapping& map, int32_t*) = nullptr;
+
+    // note: since ROS messages can be complex, using byte positions might be cumbersome to use
+    // it is easy to implement custom callbacks using ROS message types
+    // (with publish_to_topic for inputs and report_now for outputs)
+
     // offset in byte for next value (from last value) 
     std::vector<int> byte_offset;
-
     // custom conversion function used if data is not int32 and therefore
     // need to be converted to an int32 first
     // nullptr if not used
@@ -76,7 +70,6 @@ struct Mapping {
     // a pointer to an integer to increment by the amount of bytes added
     std::vector<void (*)(int32_t, byte*, int*)> converters_to_topics;
 
-    Channel channel = Channel("", 0, true); // TRON channel
     /* note: all vectors need to have the same size 
     example: 
         topic = "example_topic"
@@ -95,50 +88,78 @@ struct Mapping {
     The next position is incremented by 4 if there is no conversion function given.
     Note that the first 4 Bytes of the topics message are ignored in this example.
     */
+};
 
-    // Callback used if TRON sends some message
-    void(*input_callback)(Mapping& map, int32_t*) = nullptr;
-
-    Mapping(std::string topic, std::string channel, bool channelIsInput);
-
-    // adds variable to channel and sets position and conversion parameters
-    void add_var_to_mapping(std::string name_tron, int byte_offset = 0, 
+// sockets file descriptor, mappings, publishers and subscribers kept in struct
+struct TRON_Adapter {
+    // TRON message codes
+    static const byte GET_ERROR_MSG = 127;
+    static const byte DECL_CHAN_INPUT = 1;
+    static const byte DECL_CHAN_OUTPUT = 2;
+    static const byte ADD_VAR_TO_INPUT = 3;
+    static const byte ADD_VAR_TO_OUTPUT = 4;
+    static const byte SET_TIME_UNIT = 5;
+    static const byte SET_TIMEOUT = 6;
+    static const byte REQUEST_START = 64;
+    static const byte ANSWER_START = 0;
+    static const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set to 1
+    int socket_fd = -1;
+    std::list<Mapping> mappings;
+    std::vector<ros::Publisher> input_publishers;
+    std::vector<ros::Subscriber> output_subscribers;
+    // keep track of acknowledgements that are missing
+    /* note: since communication is asynchronous this value can only be
+    compared reliably with 0 after testing is terminated */
+    /* note: TRON only acknowledges if virtual clock is used, which it is not */
+    int acks_missing = 0; 
+
+    // creates connected socket and sets socket_fd
+    TRON_Adapter(std::string IP, uint16_t PORT);
+
+    TRON_Adapter() = default; // do not use; needed to be able to globally declare adapter instance
+
+    // declares used channel to TRON and returns Mapping to add variables
+    // when done with adding variables, add the Mapping instance to mappings list
+    Mapping createMapping(std::string topic, std::string channel, bool channelIsInput);
+
+    // declares used variable to TRON and adds given values to corresponding lists
+    void add_var_to_mapping(Mapping& map, 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);
-};
-
-// mappings, publishers and subscribers kept global
-extern std::list<Mapping> mappings;
-extern std::vector<ros::Publisher> input_publishers;
-extern std::vector<ros::Subscriber> output_subscribers;
 
-// publishes to fitting publisher from list
-template <class T>
-void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr);
-
-template<class T>
-void mapping_callback_to_topic(Mapping& map, int32_t* vars);
-
-void get_error_msg(int32_t errorcode);
-
-void add_var_to_channel(Channel& chan, bool is_input, std::string var);
-
-std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);
 
+    // sends time unit and timeout to TRON
     void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout);
 
+    // sends REQUEST_START to TRON ans waits until receiving ANSWER_START (with timeout)
     void request_start();
 
+    // reports to channel
     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);
 
+    // starts callback for every message received from TRON
+    void process_TRONs_msgs();
+
+    // template callbacks for using specified byte positions
+    template<class T>
+    void mapping_callback_to_topic(Mapping& map, int32_t* vars);
     template<class T>
     void mappings_callback_to_TRON(const ros::MessageEvent<T>& event);
 
-void process_TRONs_msgs();
+    // publishes to fitting publisher from list
+    template <class T>
+    void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr);
+
+    // gets and logs error message from TRON, then throws since proceeding would lead to more errors
+    void get_error_msg(int32_t errorcode);
+
+    // declares channel to TRON and constructs Channel instance
+    std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);
 
-void configuration_phase(ros::NodeHandle& nh);
+    // declares var to channel
+    void add_var_to_channel(Channel& chan, bool is_input, std::string var);
+};
 
 #endif //TRON_ADAPTER
 

launch/cobot1_mock_test.launch

 <launch>
 
     <include file="$(find panda_simulation)/launch/simulation.launch">
-        <arg name="gui" value="true"/>
+        <arg name="gui" value="false"/>
         <arg name="slow_simulation" value="true" />
     </include>
 

src/cobot_adapter.cpp

+#include <cooperation_msgs/PressureMsg.h>
+#include <moveit_msgs/PickupAction.h>
+#include <moveit_msgs/PlaceAction.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
+#include <moveit_msgs/MoveGroupAction.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <gazebo_msgs/LinkStates.h>
+#include <geometry_msgs/Pose.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
+#include <algorithm> // for std::find
+#include <cmath>
+#include "tron_adapter.h"
+
+TRON_Adapter adapter;
+int32_t retry_count; // set in configuration_phase
+
+void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) { 
+    if (msg->pressed)
+        adapter.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)
+        adapter.report_now("pickup_success");
+    else adapter.report_now("pickup_fail");
+}
+
+void on_place_result(const moveit_msgs::PlaceActionResult::ConstPtr &msg) {
+    if (msg->result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
+        adapter.report_now("place_success");
+    else adapter.report_now("place_fail");
+}
+
+void on_move_result(const control_msgs::FollowJointTrajectoryActionResult::ConstPtr &msg){
+    if (msg->result.error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
+        adapter.report_now("move_success");
+    else adapter.report_now("move_fail");
+}
+
+void on_move_goal(const moveit_msgs::MoveGroupActionGoal::ConstPtr &msg){
+    int32_t is_orientation_constraint_set;
+    if (msg->goal.request.path_constraints.orientation_constraints.empty())
+        is_orientation_constraint_set = 0;
+    else is_orientation_constraint_set = 1;
+    adapter.report_now("goal", 1, &is_orientation_constraint_set);
+}
+
+void on_pickup_goal(const moveit_msgs::PickupActionGoal::ConstPtr &msg){
+    int32_t is_orientation_constraint_set;
+    if (msg->goal.path_constraints.orientation_constraints.empty())
+        is_orientation_constraint_set = 0;
+    else is_orientation_constraint_set = 1;
+    adapter.report_now("goal", 1, &is_orientation_constraint_set);
+}
+
+void on_place_goal(const moveit_msgs::PlaceActionGoal::ConstPtr &msg) {
+    int32_t is_orientation_constraint_set;
+    if (msg->goal.path_constraints.orientation_constraints.empty())
+        is_orientation_constraint_set = 0;
+    else is_orientation_constraint_set = 1;
+    adapter.report_now("goal", 1, &is_orientation_constraint_set);
+}
+
+double get_pos_diff(geometry_msgs::Point p1, geometry_msgs::Point p2) {
+    double x_d = p1.x - p2.x;
+    double y_d = p1.y - p2.y;
+    double z_d = p1.z - p2.z;
+    return std::sqrt(x_d*x_d + y_d*y_d + z_d*z_d);
+}
+// factor 100 means centimeters
+int32_t get_pos_diff_int32(geometry_msgs::Point p1, geometry_msgs::Point p2, int32_t factor = 100) {
+    double pos_diff = get_pos_diff(p1, p2);
+    return std::round(pos_diff * 100);
+}
+double get_ang_diff_ignoring_yaw(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2) {
+    tf2::Quaternion q1, q2, q_diff;
+    tf2::fromMsg(q_msg1, q1);
+    tf2::fromMsg(q_msg2, q2);
+    q_diff = q1 * q2.inverse();
+    tf2::Matrix3x3 matrix(q_diff);
+    double roll, pitch, yaw;
+    matrix.getRPY(roll, pitch, yaw);
+    q_diff.setRPY(roll, pitch, 0); // set yaw to zero since it doesnt matter for glass or bottle
+    return q_diff.getAngleShortestPath();
+}
+int32_t get_ang_diff_ignoring_yaw_int32_deg(geometry_msgs::Quaternion q_msg1, geometry_msgs::Quaternion q_msg2){
+    return std::round(get_ang_diff_ignoring_yaw(q_msg1, q_msg2) * 180 / M_PI);
+}
+
+const std::vector<std::string> tron_pos_vars_ordered = {
+    "bottle_diff_to_start_pos",
+    "bottle_diff_to_start_ang",
+    "bottle_diff_to_target_pos",
+    "bottle_diff_to_target_ang",
+    "glass_diff_to_start_pos",
+    "glass_diff_to_start_ang",
+    "glass_diff_to_target_pos",
+    "glass_diff_to_target_ang"
+};
+std::vector<geometry_msgs::Pose> pose_to_compare_against; // set in configuration_phase
+bool gazebo_initialized = false;
+void on_gazebo_link_states(const gazebo_msgs::LinkStates::ConstPtr &msg){
+    int bottle_index = -1;
+    int glass_index = -1;
+    for (int j = 0; j < msg->name.size(); j++) {
+        if (msg->name[j] == "bottle::link") bottle_index = j;
+        if (msg->name[j] == "glass::link") glass_index = j;
+    }
+    if (bottle_index == -1 || glass_index == -1) return;
+    int32_t vars[tron_pos_vars_ordered.size()];
+    vars[0] = get_pos_diff_int32(msg->pose[bottle_index].position, pose_to_compare_against[0].position);
+    vars[1] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[0].orientation);
+    vars[2] = get_pos_diff_int32(msg->pose[bottle_index].position, pose_to_compare_against[1].position);
+    vars[3] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[bottle_index].orientation, pose_to_compare_against[1].orientation);
+    vars[4] = get_pos_diff_int32(msg->pose[glass_index].position, pose_to_compare_against[2].position);
+    vars[5] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[2].orientation);
+    vars[6] = get_pos_diff_int32(msg->pose[glass_index].position, pose_to_compare_against[3].position);
+    vars[7] = get_ang_diff_ignoring_yaw_int32_deg(msg->pose[glass_index].orientation, pose_to_compare_against[3].orientation);
+    for (Mapping& map : adapter.mappings) {
+        if (map.topic == "/gazebo/link_states" && !map.channel.is_input) {
+            for (int i = 0; i < tron_pos_vars_ordered.size(); i++) {
+                if (tron_pos_vars_ordered[i] == map.channel.vars[0]) {
+                    if (gazebo_initialized) {
+                        adapter.report_now(map.channel.name, 1, &vars[i]);
+                    }      
+                }
+            }
+            
+        }
+    }
+    if (!gazebo_initialized) {
+        int init_diff = 1; // position difference needed to assume gazebo is initialized
+        if (vars[0] > init_diff) return; // bottle init
+        if (vars[4] > init_diff) return; // glass init
+        gazebo_initialized = true;
+    }
+}
+
+//intentionally send not expected output to end TRON testing
+void on_test_done(Mapping& map, int32_t *ptr){
+    adapter.report_now("intentional_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);
+
+    geometry_msgs::Pose pose;
+    // bottle start
+    nh.getParam("/object/bottle/x", pose.position.x);
+    nh.getParam("/object/bottle/y", pose.position.y);
+    nh.getParam("/object/bottle/z", pose.position.z);
+    pose.orientation.w = 1.0;
+    pose_to_compare_against.push_back(pose);
+    // bottle target
+    nh.getParam("/object/bottle/place/x", pose.position.x);
+    nh.getParam("/object/bottle/place/y", pose.position.y);
+    nh.getParam("/object/bottle/place/z", pose.position.z);
+    pose.orientation.w = 1.0;
+    pose_to_compare_against.push_back(pose);
+    // glass start
+    nh.getParam("/object/glass/x", pose.position.x);
+    nh.getParam("/object/glass/y", pose.position.y);
+    nh.getParam("/object/glass/z", pose.position.z);
+    pose.orientation.w = 1.0;
+    pose_to_compare_against.push_back(pose);
+    // glass target
+    nh.getParam("/object/glass/place/x", pose.position.x);
+    nh.getParam("/object/glass/place/y", pose.position.y);
+    nh.getParam("/object/glass/place/z", pose.position.z);
+    pose.orientation.w = 1.0;
+    pose_to_compare_against.push_back(pose);
+
+
+    // 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 = adapter.createMapping("/pressure", "pressed", false);
+    adapter.add_var_to_mapping(map, "init_retry_count");
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/pressure", 10, on_pressure));
+    
+    map = adapter.createMapping("/pickup/result", "pickup_fail", false);
+    adapter.mappings.push_back(map);
+    map = adapter.createMapping("/pickup/result", "pickup_success", false);
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/pickup/result", 10, on_pickup_result));
+
+    map = adapter.createMapping("/place/result", "place_fail", false);
+    adapter.mappings.push_back(map);
+    map = adapter.createMapping("/place/result", "place_success", false);
+    adapter.mappings.push_back(map);
+    adapter.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 = adapter.createMapping("/position_joint_trajectory_controller/follow_joint_trajectory/result", "move_fail", false);
+    adapter.mappings.push_back(map);
+    map = adapter.createMapping("/position_joint_trajectory_controller/follow_joint_trajectory/result", "move_success", false);
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/position_joint_trajectory_controller/follow_joint_trajectory/result", 10, on_move_result));
+
+    map = adapter.createMapping("/move_group/goal", "goal", false);
+    adapter.add_var_to_mapping(map, "orient_constraint_set");
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/move_group/goal", 10, on_move_goal));
+
+    map = adapter.createMapping("/place/goal", "goal", false);
+    adapter.add_var_to_mapping(map, "orient_constraint_set");
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/place/goal", 10, on_place_goal));
+
+    map = adapter.createMapping("/pickup/goal", "goal", false);
+    adapter.add_var_to_mapping(map, "orient_constraint_set");
+    adapter.mappings.push_back(map);
+    adapter.output_subscribers.push_back(nh.subscribe("/pickup/goal", 10, on_pickup_goal));
+
+    for (std::string s : tron_pos_vars_ordered) {
+        map = adapter.createMapping("/gazebo/link_states", s+"_update", false);
+        adapter.add_var_to_mapping(map, s);
+        adapter.mappings.push_back(map);
+    }
+    adapter.output_subscribers.push_back(nh.subscribe("/gazebo/link_states", 10, on_gazebo_link_states));
+
+    // used to end testing when model is done
+    map = adapter.createMapping("test_done_dummy", "test_done", true);
+    map.input_callback = on_test_done;
+    adapter.mappings.push_back(map);
+    map = adapter.createMapping("test_done_dummy", "intentional_fail", false);
+    adapter.mappings.push_back(map);
+
+    // not obvious in documentation: local variables are not supported
+    // add_var_to_channel(socketfd, "ausloesen", "lokal");
+    uint64_t microseconds = 1000000; // one second
+    // documentation states 2 signed integers are used for some reason
+    adapter.set_time_unit_and_timeout(microseconds, 600);
+
+    // wait till subscribers initialized
+    for (ros::Publisher& pub : adapter.input_publishers) {
+        while (pub.getNumSubscribers() == 0) { ros::Duration(1).sleep(); };
+    }
+}
+
+int main(int argc, char**argv){
+    ros::init(argc, argv, "TRON adapter");
+    ros::NodeHandle nh;
+
+    try {
+        const std::string IP = "127.0.0.1";
+        const uint16_t PORT = 8080;
+        adapter = TRON_Adapter(IP, PORT);
+
+        configuration_phase(nh);
+        
+        adapter.request_start(); 
+        
+        // testing phase loop
+        ros::Rate test_phase_freq(10);
+        while (ros::ok()) {
+            ros::spinOnce();
+            adapter.process_TRONs_msgs();
+            test_phase_freq.sleep();
+        } 
+    } catch (const char* err){
+        ROS_FATAL("shutting down: %s", err);
+        ros::shutdown();
+    } catch (...){
+    	ROS_FATAL("shutting down");
+    	ros::shutdown();
+    }
+}