diff --git a/CMakeLists.txt b/CMakeLists.txt index aaedd5706cc6c83bfd97f7bd60acb85fe701d00e..03b84db6a340f650bbef340052da04d8087865cf 100644 --- a/CMakeLists.txt +++ b/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}) \ No newline at end of file +target_link_libraries(TRON_Adapter ${catkin_LIBRARIES} tron_adapter) \ No newline at end of file diff --git a/include/tron_adapter.h b/include/tron_adapter.h index 41bed64155c2c7aab345424c185b02ca02bcd7a3..41a44cc20c980cf83f636060602226840fd985cf 100644 --- a/include/tron_adapter.h +++ b/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); + // sends time unit and timeout to TRON + void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout); -template<class T> -void mapping_callback_to_topic(Mapping& map, int32_t* vars); + // sends REQUEST_START to TRON ans waits until receiving ANSWER_START (with timeout) + void request_start(); -void get_error_msg(int32_t errorcode); + // 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); -void add_var_to_channel(Channel& chan, bool is_input, std::string var); + // starts callback for every message received from TRON + void process_TRONs_msgs(); -std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name); + // 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 set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout); + // publishes to fitting publisher from list + template <class T> + void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr); -void request_start(); + // gets and logs error message from TRON, then throws since proceeding would lead to more errors + void get_error_msg(int32_t errorcode); -void report_now(Channel& chan, uint16_t var_count = 0, int32_t *vars = nullptr); + // declares channel to TRON and constructs Channel instance + std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name); -void report_now(std::string chan, uint16_t var_count = 0, int32_t *vars = nullptr); - -template<class T> -void mappings_callback_to_TRON(const ros::MessageEvent<T>& event); - -void process_TRONs_msgs(); - -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 diff --git a/launch/cobot1_mock_test.launch b/launch/cobot1_mock_test.launch index 2eaf8f0114bf57745c9793857e089b9dc7e198b4..59b49aabdc359d7dd088fe78aae6439d3f70625d 100644 --- a/launch/cobot1_mock_test.launch +++ b/launch/cobot1_mock_test.launch @@ -1,7 +1,7 @@ <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> diff --git a/src/cobot_adapter.cpp b/src/cobot_adapter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f3f11118c8948f9669246bade67ac86ab8066264 --- /dev/null +++ b/src/cobot_adapter.cpp @@ -0,0 +1,274 @@ +#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(); + } +} diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp index 434d027fdbf3bf357020fcda3b661abda2314f57..d6feafa71c7fa20d51aa611f9641e6122f6ecd19 100644 --- a/src/tron_adapter.cpp +++ b/src/tron_adapter.cpp @@ -3,27 +3,12 @@ #include <sys/socket.h> #include <netinet/in.h> #include <arpa/inet.h> -#include <std_msgs/Int32.h> #include <boost/make_shared.hpp> #include <memory> -#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 <fcntl.h> #include "tron_adapter.h" -const std::string IP = "127.0.0.1"; -const uint16_t PORT = 8080; - typedef uint8_t byte; // some helper functions ----------------------------------------------- @@ -86,7 +71,7 @@ void add_int32_in_network_order(int32_t num, byte *buf, int index){ inline void send_bytes(int fd, const byte *buf, int length){ byte_info(buf, length); int ret = write(fd, (void*) buf, length); - if (ret < 0) throw "writing failed"; + if (ret < 0) throw "sending failed"; } // returns false if nothing more to read and true if 4 bytes are read successfully @@ -124,43 +109,32 @@ int create_connected_socket(std::string IP, uint16_t port){ return socketfd; } -// consts for TRON ---------------------------------------------------------- -const byte GET_ERROR_MSG = 127; -const byte DECL_CHAN_INPUT = 1; -const byte DECL_CHAN_OUTPUT = 2; -const byte ADD_VAR_TO_INPUT = 3; -const byte ADD_VAR_TO_OUTPUT = 4; -const byte SET_TIME_UNIT = 5; -const byte SET_TIMEOUT = 6; -const byte REQUEST_START = 64; -const byte ANSWER_START = 0; -const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set to 1 - -// Socket kept global -int socket_fd; // ROS uses little endian for its messages const bool SYS_IS_BIG_ENDIAN = htonl(47) == 47; -Mapping::Mapping(std::string topic, std::string channel, bool channelIsInput){ - this->topic = topic; - this->channel = *send_channel_decl_msg(channelIsInput, channel).get(); + +TRON_Adapter::TRON_Adapter(std::string IP, uint16_t PORT) { + socket_fd = create_connected_socket(IP, PORT); } -std::list<Mapping> mappings; -std::vector<ros::Publisher> input_publishers; +Mapping TRON_Adapter::createMapping(std::string topic, std::string channel, bool channelIsInput){ + Mapping map; + map.topic = topic; + map.channel = *send_channel_decl_msg(channelIsInput, channel).get(); + return map; +} -void Mapping::add_var_to_mapping(std::string name_tron, int byte_offset, +void TRON_Adapter::add_var_to_mapping(Mapping& map, std::string name_tron, int byte_offset, int32_t (*conv_to_TRON)(byte*, int*), void (*conv_to_topic)(int32_t, byte*, int*)){ - add_var_to_channel(this->channel, this->channel.is_input, name_tron); - this->byte_offset.push_back(byte_offset); - this->converters_to_TRON.push_back(conv_to_TRON); - this->converters_to_topics.push_back(conv_to_topic); + add_var_to_channel(map.channel, map.channel.is_input, name_tron); + map.byte_offset.push_back(byte_offset); + map.converters_to_TRON.push_back(conv_to_TRON); + map.converters_to_topics.push_back(conv_to_topic); } - template <class T> -void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr) { +void TRON_Adapter::publish_to_topic(std::string topic, boost::shared_ptr<T> ptr) { for (ros::Publisher& pub : input_publishers) if (pub.getTopic() == topic) { pub.publish(ptr); @@ -169,48 +143,12 @@ void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr) { throw "did not find publisher for topic"; } -// callback publishing to topics like defined in mapping, used for input_callback -template<class T> -void mapping_callback_to_topic(Mapping& map, int32_t* vars){ - boost::shared_ptr<T> shared_ptr = boost::make_shared<T>(); - byte* ptr = reinterpret_cast<byte*>(shared_ptr.get()); - int next_pos = 0; - for (int i = 0; i < map.channel.vars.size(); i++){ - int32_t val = vars[i]; - next_pos += map.byte_offset[i]; - if (map.converters_to_topics[i] == nullptr) { - byte* val_bytes; - if (SYS_IS_BIG_ENDIAN){ - uint32_t switched_byte_order = htonl(*reinterpret_cast<uint32_t*>(&val)); - val = *reinterpret_cast<int32_t*>(&switched_byte_order); - } - val_bytes = reinterpret_cast<byte*>(&val); - ptr[next_pos++] = val_bytes[0]; - ptr[next_pos++] = val_bytes[1]; - ptr[next_pos++] = val_bytes[2]; - ptr[next_pos++] = val_bytes[3]; - } else { - map.converters_to_topics[i](val, &ptr[next_pos], &next_pos); - } - } - publish_to_topic<T>(map.topic, shared_ptr); -} - -// subscribers already have callbacks -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 (yet) */ -int acks_missing = 0; - -void get_error_msg(int32_t errorcode) { +void TRON_Adapter::get_error_msg(int32_t errorcode) { ROS_WARN("got error, trying to get corresponding message"); byte get_err_msg_msg[5]; - get_err_msg_msg[0] = GET_ERROR_MSG; // get error msg code + get_err_msg_msg[0] = GET_ERROR_MSG; add_int32_in_network_order(errorcode, get_err_msg_msg, 1); - send_bytes(socket_fd, get_err_msg_msg, 5); // connection is closed after this write? + send_bytes(socket_fd, get_err_msg_msg, 5); byte err_msg_length = get_bytes_socket(socket_fd, 1)[0]; auto err_msg = get_bytes_socket(socket_fd, err_msg_length); std::string msg_str = std::string(reinterpret_cast<char*>(err_msg.get()), (size_t) err_msg_length); @@ -218,7 +156,7 @@ void get_error_msg(int32_t errorcode) { throw "got error from TRON"; } -void add_var_to_channel(Channel& chan, bool is_input, std::string var) { +void TRON_Adapter::add_var_to_channel(Channel& chan, bool is_input, std::string var) { bool var_already_declared = false; for (Mapping& map : mappings) if (std::find(map.channel.vars.begin(), map.channel.vars.end(), var) != map.channel.vars.end()) var_already_declared = true; @@ -239,7 +177,7 @@ void add_var_to_channel(Channel& chan, bool is_input, std::string var) { chan.vars.push_back(var); } -std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name) { +std::unique_ptr<Channel> TRON_Adapter::send_channel_decl_msg(bool is_input, std::string name) { // prepare packet size_t msg_length = 2 + name.length(); byte msg[msg_length]; @@ -249,8 +187,7 @@ std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name) // send packet ROS_INFO("declaring channel %s as %s", name.c_str(), (is_input ? "input" : "output")); - byte_info(msg, msg_length); - write(socket_fd, (void*) msg, msg_length); + send_bytes(socket_fd, msg, msg_length); // get answer from TRON int32_t channel_identifier = get_int_socket(socket_fd); @@ -263,7 +200,7 @@ std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name) return std::make_unique<Channel>(name, channel_identifier, is_input); } -void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ +void TRON_Adapter::set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ byte msg[9]; msg[0] = SET_TIME_UNIT; byte *microseconds_bytes = reinterpret_cast<byte*>(µseconds); @@ -289,7 +226,7 @@ void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){ ROS_INFO("success: set timeout"); } -void request_start() { +void TRON_Adapter::request_start() { /* documentation confuses codes for start and getErrorMessage, actually used: 64 is start 127 is gerErrorMessage */ @@ -301,7 +238,7 @@ void request_start() { ROS_INFO("success: starting test phase"); } -void report_now(Channel& chan, uint16_t var_count, int32_t *vars){ +void TRON_Adapter::report_now(Channel& chan, uint16_t var_count, int32_t *vars){ std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(6 + 4 * var_count); add_int32_in_network_order(chan.identifier, msg.get(), 0); @@ -322,7 +259,7 @@ void report_now(Channel& chan, uint16_t var_count, int32_t *vars){ acks_missing++; } -void report_now(std::string chan, uint16_t var_count, int32_t *vars){ +void TRON_Adapter::report_now(std::string chan, uint16_t var_count, int32_t *vars){ for (Mapping& map : mappings) if (map.channel.name == chan && !map.channel.is_input) { report_now(map.channel, var_count, vars); @@ -331,34 +268,7 @@ void report_now(std::string chan, uint16_t var_count, int32_t *vars){ throw "could not report to channel"; } -// callback reports to TRON like defined in mappings -template<class T> -void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){ - std::string topic = event.getConnectionHeader().at("topic"); - byte *bytes = reinterpret_cast<byte*>(event.getMessage().get()); - for (Mapping& mapping : mappings) { - if (mapping.topic.c_str() == topic && !mapping.channel.is_input) { - int var_count = mapping.byte_offset.size(); - int32_t vars[var_count * 4]; - int next_pos = 0; - for (int i = 0; i < var_count; i++) { - next_pos += mapping.byte_offset[i]; - if (mapping.converters_to_TRON[i] == nullptr) { - if (SYS_IS_BIG_ENDIAN) - vars[i] = network_bytes_to_int_32(&bytes[next_pos]); - else vars[i] = *reinterpret_cast<int32_t*>(&bytes[next_pos]); - next_pos += 4; - continue; - } else { - vars[i] = mapping.converters_to_TRON[i](&bytes[next_pos], &next_pos); - } - } - report_now(mapping.channel, var_count, vars); - } - } -} - -void process_TRONs_msgs(){ +void TRON_Adapter::process_TRONs_msgs(){ /* note: TRONs communication after start is not guaranteed to be synchronous, thus incoming messages must be checked for their content */ while (true){ @@ -419,261 +329,56 @@ void process_TRONs_msgs(){ } } -// --------------------------------------------------------------------------------------------------------------------------------------- -// custom from here - -int32_t retry_count; // set in configuration_phase -void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) { - if (msg->pressed) - 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 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"); - else 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; - 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; - 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; - 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 : 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) { - report_now(map.channel.name, 1, &vars[i]); - } +// callback reports to TRON like defined in mappings +template<class T> +void TRON_Adapter::mappings_callback_to_TRON(const ros::MessageEvent<T>& event){ + std::string topic = event.getConnectionHeader().at("topic"); + byte *bytes = reinterpret_cast<byte*>(event.getMessage().get()); + for (Mapping& mapping : mappings) { + if (mapping.topic.c_str() == topic && !mapping.channel.is_input) { + int var_count = mapping.byte_offset.size(); + int32_t vars[var_count * 4]; + int next_pos = 0; + for (int i = 0; i < var_count; i++) { + next_pos += mapping.byte_offset[i]; + if (mapping.converters_to_TRON[i] == nullptr) { + if (SYS_IS_BIG_ENDIAN) + vars[i] = network_bytes_to_int_32(&bytes[next_pos]); + else vars[i] = *reinterpret_cast<int32_t*>(&bytes[next_pos]); + next_pos += 4; + continue; + } else { + vars[i] = mapping.converters_to_TRON[i](&bytes[next_pos], &next_pos); } } - + report_now(mapping.channel, var_count, vars); } } - 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){ - 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 = 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); - 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)); - - 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); - 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)); - - map = Mapping("/move_group/goal", "goal", false); - map.add_var_to_mapping("orient_constraint_set"); - mappings.push_back(map); - output_subscribers.push_back(nh.subscribe("/move_group/goal", 10, on_move_goal)); - - map = Mapping("/place/goal", "goal", false); - map.add_var_to_mapping("orient_constraint_set"); - mappings.push_back(map); - output_subscribers.push_back(nh.subscribe("/place/goal", 10, on_place_goal)); - - map = Mapping("/pickup/goal", "goal", false); - map.add_var_to_mapping("orient_constraint_set"); - mappings.push_back(map); - output_subscribers.push_back(nh.subscribe("/pickup/goal", 10, on_pickup_goal)); - - for (std::string s : tron_pos_vars_ordered) { - map = Mapping("/gazebo/link_states", s+"_update", false); - map.add_var_to_mapping(s); - mappings.push_back(map); - } - output_subscribers.push_back(nh.subscribe("/gazebo/link_states", 10, on_gazebo_link_states)); - - // used to end testing when model is done - map = Mapping("test_done_dummy", "test_done", true); - map.input_callback = on_test_done; - mappings.push_back(map); - map = Mapping("test_done_dummy", "intentional_fail", false); - 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 - set_time_unit_and_timeout(microseconds, 600); - - // wait till subscribers initialized - for (ros::Publisher& pub : 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 { - socket_fd = create_connected_socket(IP, PORT); - - configuration_phase(nh); - request_start(); - - // testing phase loop - ros::Rate test_phase_freq(10); - while (ros::ok()) { - ros::spinOnce(); - 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(); +// callback publishing to topics like defined in mapping, used for input_callback +template<class T> +void TRON_Adapter::mapping_callback_to_topic(Mapping& map, int32_t* vars){ + boost::shared_ptr<T> shared_ptr = boost::make_shared<T>(); + byte* ptr = reinterpret_cast<byte*>(shared_ptr.get()); + int next_pos = 0; + for (int i = 0; i < map.channel.vars.size(); i++){ + int32_t val = vars[i]; + next_pos += map.byte_offset[i]; + if (map.converters_to_topics[i] == nullptr) { + byte* val_bytes; + if (SYS_IS_BIG_ENDIAN){ + uint32_t switched_byte_order = htonl(*reinterpret_cast<uint32_t*>(&val)); + val = *reinterpret_cast<int32_t*>(&switched_byte_order); + } + val_bytes = reinterpret_cast<byte*>(&val); + ptr[next_pos++] = val_bytes[0]; + ptr[next_pos++] = val_bytes[1]; + ptr[next_pos++] = val_bytes[2]; + ptr[next_pos++] = val_bytes[3]; + } else { + map.converters_to_topics[i](val, &ptr[next_pos], &next_pos); + } } -} + publish_to_topic<T>(map.topic, shared_ptr); +} \ No newline at end of file